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ABSTRACT 


This  thesis  develops  an  extended  Kalman  filter  for  real-time  estimation  of  rigid 
body  motion  attitude.  The  filter  represents  rotations  using  quaternions  rather  than  Euler 
angles,  which  eliminates  the  long-standing  problem  of  singularities  associated  with  those 
angles.  A  process  model  for  rigid  body  angular  motions  and  angular  rate  measurements  is 
defined.  The  process  model  converts  angular  rates  into  quaternion  rates,  which  are  in  turn 
integrated  to  obtain  quaternions.  The  outputs  of  the  model  are  values  of  three- 
dimensional  angular  rates,  three-dimensional  linear  accelerations,  and  three-dimensional 
magnetic  field  vector.  Gauss-Newton  iteration  is  utilized  to  find  the  best  quaternion  that 
relates  the  measured  linear  accelerations  and  earth  magnetic  field  in  the  body  coordinate 
frame  to  calculated  values  in  the  earth  coordinate  frame.  The  quaternion  obtained  from 
the  optimization  algorithm  is  used  as  part  of  the  observations  for  the  Kalman  filter.  As  a 
result,  the  measurement  equations  become  linear. 

A  new  approach  to  attitude  estimation  is  introduced  in  this  thesis.  The 
computational  requirements  related  to  the  extended  Kalman  filter  developed  using  this 
approach  are  significantly  reduced,  making  it  possible  to  estimate  attitude  in  real-time. 
Extensive  static  and  dynamic  simulation  of  the  filter  using  Matlab  proved  it  to  be  robust. 
Test  cases  included  the  presence  of  large  initial  errors  as  well  as  high  noise  levels.  In  all 
cases  the  filter  was  able  to  converge  and  accurately  track  attitude. 
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EXECUTIVE  SUMMARY 


This  thesis  presented  a  complete  design  of  an  Extended  Kalman  filter  for  real¬ 
time  estimation  of  rigid  body  motion  attitude.  The  use  of  quaternions  to  represent 
rotations,  instead  of  Euler  angles,  eliminated  the  long-standing  problem  of  singularities 
called  “gimbal  lock”. 

A  simple  first-order  process  model  for  rigid  body  angular  rate  measurements  was 
defined,  which  closely  matched  real  process  data  obtained  from  angular  rate  sensors. 

The  quaternion  rates  were  nonlinearly  related  to  the  current  quaternion  and  the 
angular  rates.  The  process  model  converted  angular  rates  into  quaternion  rates,  which 
were  integrated  to  obtain  quaternions. 

Two  approaches  to  the  Kalman  filter  design  were  investigated.  The  first  approach 
used  nine  output  equations:  three  angular  rates,  three  components  of  linear  acceleration, 
and  three  components  of  the  earth  magnetic  field.  Since  these  output  equations  were 
highly  nonlinear  functions  of  the  process  state  variables,  the  partial  derivatives  needed 
for  the  Kalman  filter  design  were  very  complicated.  As  a  result,  the  Kalman  filter  was 
not  feasible  for  real-time  implementation. 

The  second  approach  utilized  Gauss-Newton  algorithm  to  find  the  optimal 
quaternion  that  related  the  values  of  linear  accelerations  and  earth  magnetic  field  in  the 
body  coordinate  frame  and  in  the  earth  coordinate  frame.  The  optimal  quaternion  was 
used  as  part  of  the  measurement  for  the  Kalman  filter,  .which  had  seven  output  equations: 
three  angular  rates  and  four  components  of  quaternions.  Furthermore,  the  output 
equations  were  linear  and  the  output  matrix  was  the  identity  matrix.  As  a  result,  the 
partial  derivatives  were  total  derivatives  and  had  constant  values.  The  convergence 
algorithm  replaced  the  computation  of  the  partial  derivatives  of  the  nine  nonlinear 
measurement  equations.  The  following  diagram  represents  the  approach. 
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The  computational  requirements  for  the  Kalman  filter  developed  using  this 
approach  were  significantly  reduced,  making  it  possible  to  estimate  attitude  in  real  time. 

Extensive  static  and  dynamic  simulation  of  the  filter  using  Matlab  proved  it  to  be 
robust.  Test  cases  included  the  presence  of  large  initial  errors  as  well  as  high  noise  levels. 
In  all  cases  the  filter  was  able  to  converge  and  accurately  track  attitude. 


XIV 


ACKNOWLEDGMENTS 


I  would  like  to  thank  Dr.  Xiaoping  Yun,  who  gave  me  the  opportunity  to  work  on 
this  difficult  and  interesting  topic.  His  assistance  and  patience  enabled  me  to  present 
difficult  concepts  in  this  thesis  in  a  meaningful  manner. 

I  wish  to  express  my  sincere  appreciation  to  Prof.  Eric  Bachman  for  his  advice 
and  suggestions  during  the  process  of  writing  and  presenting  my  thesis.  I  know  how 
important  time  is  for  a  doctor’s  degree  candidate. 

I  express  my  gratitude  to  Dr.  Hutchins,  the  first  person  who  taught  me  how 
quaternions  work.  In  his  class  I  developed  my  first  Extended  Kalman  filter  and  learned 
all  the  background  needed  for  this  thesis. 

I  specially  thank  Dr.  Robert  McGhee  for  the  original  vision  of  the  quaternion 
convergence.  His  idea  is  the  core  of  this  thesis  and  led  us  all  to  great  discoveries. 

Finally,  I  would  like  to  thank  my  wife,  Nice,  for  her  love,  support,  patience  and 
faith  throughout  this  time.  I  would  also  like  to  thank  my  children,  Andre  and  Amanda, 
whose  love  and  very  presence  were  a  continual  source  of  happiness  and  strength. 


XV 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 


XVI 


I.  INTRODUCTION 


A.  MOTIVATION 

Navigation  is  the  process  of  computing  the  position,  orientation  and  speed  of  a 
body  or  vehicle.  When  this  computation  is  based  solely  on  inputs  from  self-contained 
inertial  sensing  instruments  it  is  called  inertial  navigation.  Angular  rate  sensors  and 
accelerometers  are  typical  inertial  sensors. 

Inertial  navigation  systems  integrate  the  output  of  accelerometers  to  obtain 
velocity,  and  integrate  velocity  to  obtain  distance  traveled.  In  order  to  accomplish  this, 
the  system  should  know  the  orientation  of  the  accelerometer  axes  relative  to  a  fixed 
reference  frame.  Whether  this  orientation  is  fixed  or  not,  leads  to  two  different  types  of 
mechanizations  for  inertial  systems.  These  mechanizations  establish  how  the  sensors  are 
attached  to  the  system. 

In  the  first  type,  the  inertial  sensors  are  mounted  on  a  stable  platform,  which  is 
connected  to  gimbals.  Torque  motors  receive  information  from  the  angular  rate  sensors 
and  drive  the  gimbals  in  order  to  keep  the  platform  leveled  and  headed  in  a  fixed 
direction  (usually  north,  east,  and  down  -  NED).  The  position  is  calculated  using  the 
output  of  accelerometers  directly.  This  kind  of  mechanization  is  called  "gimbaled". 

Alternatively,  the  inertial  sensors  can  be  fixed  to  the  vehicle.  This  mechanization 
is  called  "strapdown".  In  this  case,  the  accelerations  are  measured  with  respect  to  body- 
fixed  axes  and  they  need  to  be  converted  into  integration  axes,  normally  earth-fixed  axes. 
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Thus,  it  is  important  to  keep  track  of  the  orientation  of  the  body  with  respect  to  the  earth 
coordinates. 

Orientation  can  be  defined  as  a  set  of  parameters  that  relates  the  angular  position 
of  a  frame  to  another  reference  frame.  There  are  numerous  methods  for  describing  this 
relation.  Among  them,  rotation  matrices,  Euler  angles  and  quaternions  are  commonly 
used.  In  “strapdown”  systems,  they  must  be  calculated  at  each  step  of  the  integration,  so 
the  coordinate  transformation  between  frames  can  be  carried  out.  Orientation  or  attitude 
tracking  systems  perform  this  task.  They  are  used  in  navigation  systems,  mobile  robots, 
head  tracking  and  other  applications. 

Attitude  tracking  systems  might  be  implemented  in  different  forms.  [Ref.  1] 
presents  a  solution  using  GPS  and  inertial  sensors  used  for  aircraft.  The  difference  among 
the  GPS  signals  received  by  three  antennas  gives  attitude  information.  [Ref.  2]  replaces 
the  antenna  information  with  information  coming  from  celestial  observations.  [Ref  3] 
describes  an  attitude  package,  which  combines  the  outputs  of  inclinometers,  gyros,  and 
compasses  to  obtain  attitude  estimation.  All  three  examples  utilize  Euler  angles  to 
represent  orientation  and  a  Kalman  filtering  algorithm  to  integrate  the  information. 

The  system  in  [Ref.  4]  has  a  different  approach.  It  uses  what  is  called  Magnetic 
Angular  Rate  Gravity  (MARG)  sensors.  Each  MARG  sensor  consists  of  three  orthogonal 
rate  sensors,  three  orthogonal  accelerometers,  and  three  orthogonal  magnetometers 
mounted  in  a  “strapdown”  configuration.  Quaternions  represent  the  orientation  between 
frames.  The  use  of  quaternions  avoids  the  singularity  problems  characteristic  of  filters 
that  use  Euler  angles.  Integration  of  angular  rate  outputs  provides  an  angular  orientation, 
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which  is  corrected  with  information  coming  from  the  accelerometers  and  magnetometers. 
A  complementary  filter  estimates  the  attitude  of  a  rigid  body  that  the  sensor  is  attached 
to.  Complementary  filters  are  used  when  the  system  deals  with  redundant  measurement 
of  same  quantity  corrupted  with  noises  separated  in  frequency  (low-frequency  and  high- 
frequency).  They  have  fixed  gains  and  normally  are  designed  in  the  frequency-domain. 

This  thesis  intends  to  follow  the  same  approach  described  above,  but  replaces  the 
complementary  filter  with  a  Kalman  filter.  The  Kalman  filter  blends  information  from  the 
sensors  to  provide  an  optimal  estimate  from  all  sources  combined.  Instead  of  fixed  gains, 
the  gains  of  an  Kalman  filter  are  adjusted  at  each  step. 

The  use  of  quaternions  is  maintained  because  of  characteristics  that  make  them 
very  suitable  for  this  approach.  They  can  easily  be  transformed  into  a  matrix  and  can  be 
integrated  easily  due  to  their  dependence  on  the  angular  rates.  They  are  quadratic 
functions,  which  guarantees  good  convergence  properties.  These  characteristics  may  be 
used  to  reduce  the  order  of  the  output  vector  as  well  as  the  computational  effort  needed  to 
run  the  filter. 

B.  RESEARCH  QUESTIONS 

This  thesis  will  examine  the  following  research  topics: 

-  Evaluate  the  use  of  quaternions  for  representing  attitude  in  conjunction  with 
attitude  filter  design; 

-  Design  a  Kalman  Filter  that  estimates  attitude  based  on  information  provided 
by  accelerometers,  angular  rates,  and  magnetometers; 
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Verify  the  viability  of  using  the  quadratic  properties  of  quaternions  to  reduce 
the  filter  complexity  and  computational  requirements;  and 

-  Test  the  filter  performance  using  simulated  data. 

C.  THESIS  OUTLINE 

The  purpose  of  this  thesis  is  to  present  a  complete  study  and  implementation  in 
Matlab  of  a  Kalman  Filter  that  estimates  the  attitude  of  a  body  with  respect  to  a  fixed 
reference  frame.  The  parameters  that  represent  attitude  are  quaternions.  The  thesis  is 
organized  as  follows: 

-  Chapter  II  reviews  the  concepts  of  frames  and  compares  several  methods  for 
representing  frame  rotations.  It  introduces  the  notion  of  quaternions  and 
shows  how  they  can  represent  rotations.  It  also  reviews  the  basics  of  Kalman 
Filters. 

-  Chapter  HI  presents  the  process  model  used  to  develop  the  Kalman  filter.  As 
the  quaternion  rate  depends  on  the  values  of  angular  rates,  the  model  utilized 
in  this  thesis  is  based  on  the  relationship  between  angular  rates  and 
quaternions. 

Chapter  IV  is  the  core  of  this  thesis.  It  starts  with  the  usual  formulation  of  the 
Kalman  Filter.  Subsequently,  a  more  efficient  method  is  presented,  which 
reduces  the  order  of  the  output  vector  and  results  in  a  linear  state-output 
relationship.  The  link  between  these  two  approaches  is  the  convergence 
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properties  of  quaternions.  Solutions  based  on  the  Gauss  and  Gauss-Newton 
convergence  algorithms  are  presented. 

Chapter  V  describes  the  procedure  applied  to  obtain  the  statistical  properties 
of  the  system. 

Chapter  VI  presents  tests  of  the  convergence  properties  and  describes  the  filter 
simulation.  It  explains  how  simulated  data  were  created  and  discusses  the 
results  of  applying  that  data  to  the  filter.  Static  and  dynamic  tests  were  used  to 
evaluate  the  filter  performance. 

Chapter  VII  presents  a  short  summary  along  with  an  analysis  of  results  and 
conclusions.  It  ends  with  suggestions  for  the  future  work. 
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II.  BACKGROUND 


A.  INTRODUCTION 

There  are  several  conventions  for  describing  orientation  and  rotation  in  3D  space. 
Some  are  easier  to  visualize  than  others  are.  Each  has  some  kind  of  limitation.  This 
chapter  presents  the  notion  of  a  frame  and  gives  an  overview  on  three  well-known 
orientation  or  rotation  methods.  Quaternions  are  introduced  as  an  alternative  method. 

It  also  presents  the  basics  of  the  Kalman  filter  by  discussing  the  formulation  and 
structure  of  the  discrete  Kalman  filter,  which  will  be  used  in  this  thesis. 

B.  ROTATING  FRAMES 

1.  Frames 

In  order  to  describe  the  orientation  of  a  body,  a  coordinate  system  is  attached  to 
the  body  and  then  it  is  referenced  to  another  fixed  coordinate  system.  If  the  Cartesian 
coordinate  system  is  adopted,  a  set  of  three  orthonormal  vectors  is  used.  This  set  of 
vectors  is  called  a  frame. 

Two  frames  are  used  in  this  thesis.  The  earth  frame  is  fixed  and  has  its  x,  y,  and  z- 
axes  pointing  North,  East,  and  Down  respectively.  The  body  frame  moves  with  the  body 
being  tracked  and  has  its  x,  y,  and  z-axes  parallel  to  the  main  axes  of  the  body.  The  body 
axes  point  respectively  to  the  front,  right,  and  down. 
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Any  vector  can  be  represented  in  a  frame  by  three  components.  These 
components  are  the  projections  of  the  vector  along  each  of  the  frame  axes.  Given  the 
components  of  a  vector  in  one  frame,  the  components  in  another  frame  can  be  calculated 
provided  that  the  relationship  between  the  frames  is  known. 

a.  Rotation  Matrices 

Let  Bv  and  £v  be  3  x  1  vectors  containing  the  components  of  a  reference 
vector  in  frames  B  and  E.  There  exists  a  3  x  3  matrix  R  such  that 

ev=Rbv,  (2.1) 

where  each  column  of  R  is  the  projection  of  each  one  of  the  frame-5  axes  on  the  frame-£ 
axes. 

Rotation  matrices  must  be  orthogonal  (both  frames  with  orthogonal  axes)  and 
orthonormal  (all  axes  are  orthonormal).  Thus,  although  the  matrix  has  nine  elements 
these  constraints  reduce  the  number  of  degrees-of-freedom  (DOF)  to  three. 

Using  rotation  matrices  has  some  drawbacks  [Ref.  5].  Violation  of  the  above 
constraints  during  the  execution  of  the  algorithm  may  make  it  difficult  to  keep  the  matrix 
orthonormalized.  Another  problem  is  that  it  is  very  hard  to  interpolate  rotations  between 
two  orientations. 

b.  Euler  Angles 

Suppose  that  a  sequence  of  rotations  is  applied  to  a  frame  B  about  its  own 
axes.  The  Euler  Theorem  states  that  three  rotations  are  needed  (no  two  rotations  about  the 
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same  axis)  to  make  the  frame  B  coincide  with  another  frame  E  [Ref.  6].  The  three  angles 
are  called  Euler  Angles. 

It  is  worth  noting  that  each  rotation  is  about  the  new  coordinate  frame  obtained 
after  the  previous  rotation.  For  example,  a  rotation  sequence  xyz  means  a  rotation  about  x- 
axis,  followed  by  a  rotation  about  the  new  y-axis,  followed  by  a  rotation  about  the  newer 
z-axis. 

There  is  one  rotation  matrix  that  represents  each  rotation  described  above.  This 
compound  rotation  is  the  product  of  those  three  matrices  and  it  is  the  same  rotation 
matrix  mentioned  in  the  previous  section. 

This  method  of  describing  orientation  is  efficient  because  it  uses  only  three  angles 
to  represent  three  degrees-of-freedom.  Euler  angles  are  not  constrained,  so  they  don’t 
have  to  be  adjusted  (see  quaternion  normalization).  However,  although  only  three  angles 
represent  the  orientation,  it  may  be  necessary  to  construct  a  rotation  matrix  for  each  angle 
before  performing  a  rotation. 

Other  problems  arise  when  using  Euler  angles  [Ref.  5].  It  is  difficult  to  find  a 
series  of  rotations  that  represent  a  single  rotation.  Furthermore,  Euler  angles  have  a 
singularity  problem  named  "gimbal  lock".  It  happens  when  the  coordinate  frame  is 
rotated  90  degrees  consecutively. 

For  instance,  suppose  a  frame  with  three  orthonormal  axes  is  rotated  90  degrees 
about  the  x-axis  and  90  degrees  about  the  new  y-axis.  The  old  x  axis  and  the  newer  z  axis 
are  now  aligned  and  one  degree-of-freedom  is  lost.  In  this  case,  the  rotations  about  the  x 
and  z  axes  are  no  longer  unique  and  only  their  sum  or  difference  can  be  described. 
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c.  Axis  and  Angle  Rotation 

This  method  is  based  on  the  concept  of  the  Euler  axis  of  rotation.  The 
Euler  axis  is  defined  as  the  axis  of  rotation  about  which  one  coordinate  frame  can  be 
rotated  into  another.  In  other  words,  there  exists  a  pair  composed  of  an  axis  and  an  angle 

(B  v,0)  such  that  if  a  frame  B  is  rotated  about  this  axis  through  an  angle  0  the  final 
position  of  frame  B  coincides  with  the  position  of  frame  E.  Note  that  the  axis  is 
represented  in  the  original  frame  B. 

This  method  also  has  the  problems  cited  in  the  previous  sections  with  the 
exception  of  "gimbal  lock".  It  is  a  step  in  the  direction  of  another  way  to  represent 
rotations,  namely,  quaternions. 

2.  Quaternions 

The  attitude  reference  quaternion  is  a  different  way  of  representing  the  Euler  axis 
and  angle.  It  uses  a  set  of  four  parameters:  three  represent  the  components  of  a  vector 
directed  along  the  Euler  axis;  the  fourth  is  a  scalar  quantity. 

The  discussion  to  follow  describes  how  quaternions  are  used  as  an  alternative  to 
Euler  angles  in  strapdown  attitude  systems. 

a.  Motivation 

Understanding  how  quaternions  perform  rotation  is  quite  easy  if  the 
reasoning  starts  in  two  dimensions  [Ref.  7].  Let  v  be  a  complex  number 

v  =  ei  +  h,  (2.2) 
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where  i  represents  the  imaginary  number  so  that 

ii  =  - 1  (2.3). 

The  complex  number  v  can  be  thought  of  as  a  vector  with  two  components  e  and  h 
in  the  complex  plane. 

Define  another  complex  number  u  =  a  i+d  with  components 


a  =  sin  # 
d  =  cos  # 


(2.4). 


The  product  of  u  and  v  is  given  by 

w  =  u  v  =  (e  cosd  +  h  sin#)  i  +  (h  cos#  — e  sin#)  (2.5). 

From  the  latter  expression  it  is  easy  to  notice  that  the  product  vector  represents  the 
vector  v  rotated  by  #  in  the  complex  plane  [Ref.  7]. 


b.  Quaternions 

In  1843,  Hamilton  invented  the  so-called  hyper-complex  number  of  rank 
4,  to  which  he  gave  the  name  quaternion.  Quaternions  are  a  four-dimensional  extension 
to  complex  numbers.  Based  on  Equation  2.3,  Hamilton  defined  a  similar  rule  for  dealing 
with  the  operations  on  the  vector  part  of  the  quaternion.  He  stated  that 

i2  =  j2=k2=ijk  =  -\  (2.6). 

A  quaternion  can  be  regarded  as  an  element  of  SR4 .  In  this  thesis,  quaternions  will 
be  represented  using  the  notation  from  [Ref.  7],  which  defines  the  quaternions  as 

n  =  a  i+b  j  +  ck  +  d,  (2.7) 
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where  a,  b,  c,  d  are  real  numbers  and  i,j,  k  are  unit  vectors  directed  along  the  x,  y,  and  z 
axis  respectively. 

An  alternative  way  of  representing  a  quaternion  is  as  the  sum  of  a  scalar  and  a  3- 
dimensional  vector  [Ref.  6].  The  Equation  2.7  can  be  rewritten  as  follows 

n  =  n0  +  n,  (2.8) 


where  the  vector 


n  =  (a,b,c)  = 

When  the  scalar  part  of  a  quaternion  is  zero  the  quaternion  is  called  a  pure 
quaternion. 

The  product  of  two  quaternions  is  defined  as  follows. 

Let  n  and  m  be  two  quaternions  such  that 

n  =  ai+b  j+ck+d  =  n0+n  (2.10) 

m-ei  +  f  j  +  g  k  +  h  =  m0+m  (2.11). 


a 

0 

b 

and  n0=d  = 

0 

c 

0 

0 

d 

(2.9). 


The  quaternion  product  of  n  and  m  is  defined  so  that  the  following  special  products  are 
satisfied  [Ref.  6]: 


i 2  =  / 2  =  k2  =  i  j  k  =  —  1 

(2.12) 

ij  =  k  =  -ji 

(2.13) 

1 

II 

n 

(2.14) 

k  i  =  j  ~  —i  k 

(2.15). 
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Note  that  Equations  2.12  to  2.15  are  the  usual  vector  product  for  tri-orthogonal 
vectors.  Then,  applying  all  the  above  rules  and  the  associative  and  commutative  laws  of 
scalar  multiplication,  the  product  of  m  and  n  is 

nm-d  h-{ae+b  f+c  g)+ 

+d(ei+f  j+g  k)+h(ai+b  j+ck)+  (2.16). 

+  {b  g-c  f)  i  +  (ce-a  g)  j  +  {af  -b  e)  k 

After  separating  the  scalar  and  vector  products,  the  above  equation  can  be  written 
nm  =  n0m0-nm  +  nQm  +  m0n  +  n  xiit.  (2.17). 

Note  that  the  vector  cross  product  that  appears  in  the  last  term  makes  the 
quaternion  product,  in  general,  non-commutative. 

Another  application  for  the  notation  “scalar  plus  vector”  is  the  rotation  using 
quaternions,  which  is  focused  on  the  next  section. 


c.  Quaternions  Representing  Rotations 
A  quaternion 

n  =  ai+b  j+ck  +  d  =  n0+n,  (2.18) 


such  that 

n0  =  cos  6  and  |n|  =  sin£,  (2. 19) 

can  be  used  to  rotate  a  vector  u .  Note  that  these  two  constraints  make  n  a  unit 
quaternion. 

In  order  to  do  that,  u  must  be  written  as  a  pure  quaternion 

u  —  Uq  +u  =0+u  (2.20). 
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The  rotation  is  performed  through  the  double  quaternion  multiplication 

nun,  (2.21) 

where  n*is  the  complex  conjugate  of  the  quaternion  n  defined  as 

n"=-ai-bj-ck+d  =  n0-n  (2.22). 

This  operation  rotates  the  vector  u  through  an  angle  26  about  the  axis  defined  by 
n  or,  similarly,  rotates  the  frame  through  an  angle  -  26  about  the  axis  defined  by  n . 

The  operation  using  Equation  2.21  is  equivalent  to  a  matrix  multiplication 

nun  =Ru,  (2.23) 


where 

d2+a2-b2-c2  2(ab-cd )  2  {ac+bd) 

R=  2  (ab+cd)  d2+b2-a2-c2  2{bc-ad)  (2.24). 

2  (ac-bd)  2  (bc+ad)  d2+c2-b2-a 2 

In  both  cases  it  can  be  proved  [Ref.  3]  that  the  result  is  again  a  pure  quaternion, 
which  means  it  represents  a  vector.  Furthermore,  the  final  vector  has  the  same  length  as 
the  initial  one. 

As  a  further  example,  suppose  that  a  vector  v  pointing  in  the  x-axis  direction  is  to 
be  rotated  120  degrees  about  the  axis  defined  by  (i,  j,k )  =  (1,1,1). 

First,  v  should  be  written  as  a  pure  quaternion  v 

v  =  (l,  0,0,0). 

The  unit  vector  representing  the  Euler  axis  is 


s's's; 
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This  unit  vector  and  the  rotation  angle  are  used  to  calculate  the  unit  quaternion 


and  its  complex  conjugate 


f 

-f1 1  1  M 

n  = 

sin  60° 

V 

,  cos  60° 

J 

r\2,2,2’2  J 

n  = 


Finally,  the  vector  v  is  rotated  using  Equation  2.24 
Rv  =  n  u  n  =(  0 , 1,0,0). 

As  can  be  seen  in  Figure  2.1  a  rotation  of  120  degrees  about  the  referred  axis 
moves  the  vector  v  in  x-axis  to  a  new  position  in  y-axis. 


Figure  2.1.  Rotation  Using  Quaternions. 
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Quaternion  rotation  operators  have  no  singularities  and  they  can  relate  the 
components  of  a  vector  when  it  is  represented  in  any  two  independent  coordinate  frames 

in  9t3. 

C.  THE  KALMAN  FILTER 

1.  Basics 

The  attitude  of  a  body  can  be  determined  by  either  deterministic  or  non- 
deterministic  methods  [Ref.  8]. 

Deterministic  models  use  two  vectors  representing  the  measurement  in  two 
different  frames  and  calculate  the  matrix  that  transforms  one  coordinate  system  into 
another.  For  one  measurement  in  each  frame  and  no  noise,  the  problem  can  be 
represented  by  a  system  of  equations  and  easily  solved.  If  there  exists  more  than  one 
measurement  in  one  of  the  frames,  which  includes  measurement  noise,  the  solution  is  not 
unique  and  optimization  methods  must  be  used  in  order  to  find  the  best  solution  with 
minimum  error. 

Non-deterministic  methods  combine  dynamic  and/or  kinematics  models  with 
sensor  data.  Estimation  algorithms  utilize  a  time  series  of  measurements  in  order  to 
estimate  the  attitude  of  the  body.  Among  them,  the  Kalman  Filter  has  been  proven  to  be 
very  useful  for  attitude  estimation  using  vector  measurements. 
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2. 


The  Kalman  Filter 


The  general  Kalman  filter  structure  can  be  applied  to  a  variety  of  problems.  One 
of  the  principal  advantages  of  the  filter  is  that  it  allows  the  blending  of  information  from 
several  different  sources  to  develop  the  optimum  estimate.  The  resulting  estimate  is 
statistically  optimal  with  respect  to  the  input  data. 

Kalman  filters  compute  the  solution  recursively.  In  particular,  each  updated 
estimate  of  the  state  is  computed  from  the  previous  estimate  and  the  new  input  data,  so 
only  the  previous  estimate  requires  storage.  Thus,  the  Kalman  Filter  is  computationally 
more  efficient  than  others  filters  that  require  all  previous  input  data  at  each  step  of  the 
filtering  process  and  it  becomes  ideal  for  implementation  on  a  digital  computer. 

3.  The  Filter  Structure 

The  Kalman  Filter  is  a  filter  that  can  optimally  estimate,  in  real  time,  the  states  of 
a  system  based  on  noise  corrupted  inputs.  State  variables  describe  system  behavior  as  a 
function  of  time.  Errors  in  modeling  the  system  are  treated  as  process.  The  noise  is 
assumed  to  be  a  Gaussian  random  process  with  a  known  covariance  matrix  [Ref.  8]. 

The  process  noise  is  used  to  focus  on  the  measurements  more  than  on  the  model 
itself.  The  filter  minimizes  the  covariance  of  the  estimated  error  between  the  model 
output  and  the  measurements. 
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4.  The  Discrete  Kalman  Filter  Algorithm 

The  following  description  of  the  Kalman  filter  is  based  on  [Ref.  9].  It  assumes  a 
process  model  with  n  states.  The  equation  describing  how  the  process  evolves  in  time  is 

xk+i  =Fk  xk  (2-25). 

The  measurement  equations  define  how  the  outputs  are  related  to  the  states.  For  a 
system  with  m  outputs  the  measurement  equation  is 

Zk  =Hk  xk+vk  (2.26). 

Each  term  of  Equations  (2.25)  and  (2.26)  an  be  defined  as  follows. 
xk  :  (n  x  1)  process  state  vector  at  time  tk 

F(xk)  :  (n  x  n)  matrix  relating  the  states  xk  to  xk+l  (state  transition  matrix) 

wk  :  (n  x  1)  process  noise  vector  with  known  covariance  structure  at  time  tk 

zk  :  (m  x  1)  measurement  vector  at  time  tk 

H(xk) :  (m  x  n)  matrix  relating  the  states  xk  to  the  measurement  yk 

v*  :  (m  x  1)  measurement  noise  vector  with  known  covariance  structure  at  time  tk 

The  following  covariance  matrices  are  assumed  for  the  noise  vectors 


r  \Qk,  i  =  k 
^Ho.  i*k 

(2.27) 

T  f/Ji,  i  =  k 

(2.28) 

E[wkvJ  ]  =  0,  for  all  i,k 

(2.29). 
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The  discrete  Kalman  Filter  contains  five  recursive  equations.  A  prior  estimate  of 
the  state  is  updated  using  a  correction  proportional  to  the  difference  between  the 
estimated  output  and  the  observation 

(2.30). 

The  proportionality  factor  Kk  is  known  as  Kalman  gain.  This  gain  is  calculated 
such  that  the  trace  of  the  covariance  matrix  for  the  states  is  minimized  is  given  by 

Kk  --  p;  Hi  (ff,  p;  Hi  +  Rt )"' ,  (2.3i) 

where  Pk  is  the  covariance  matrix  for  the  state  x~k . 

For  the  updated  estimate  of  the  state  xk ,  a  new  covariance  matrix  can  be 
computed 

Pk=  (l-K,Ht)Pt-  (2.32). 

The  updated  estimated  is  then  projected  ahead  using  the  state  transition  matrix 

*~k+i=Fkxk  (2.33). 

Finally,  the  covariance  matrix  for  the  projected  state  is  calculated  as 

flT.,=  FkPk  Fkr+Qk  (2.34). 

The  complete  loop  of  the  algorithm  can  be  seen  in  Figure  2.2. 


19 


Enter  prior  estimate  xq  and 
its  error  cov  ariance  Pq 


Figure  2.2.  Kalman  Loop.  From  [Ref.  9]. 
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in.  THE  MODEL 


A.  INTRODUCTION 

This  chapter  presents  the  steps  one  needs  to  carry  out  before  designing  a  Kalman 
filter.  It  starts  by  defining  the  state  variables.  Characteristics  of  the  sensor  used  make  it 
necessary  to  include  quaternion  components  as  states.  The  relationship  between 
quaternions  and  angular  rates  is  derived,  which  allows  the  development  of  a  process 
model  for  the  complete  system. 

B.  THE  PROCESS  MODEL 

In  order  to  start  designing  a  Kalman  filter,  three  steps  must  be  completed: 

-  Choose  the  states  of  the  system.  The  states  or  state  variables  are  variables 
representing  the  dynamics  of  the  system.  The  Kalman  filter  is  designed  to 
estimate  these  variables; 

-  Choose  the  output  of  the  system.  The  outputs  are  variables  related  to  the 
states,  which  are  compared  to  observed  values; 

-  Define  a  process  model  for  the  state  variables.  A  process  model  is  a 
mathematical  model  that  has  white  noise  as  an  input  and  a  signal  with  the 
same  characteristics  as  the  state  variables  in  the  output.  In  general,  this  model 
is  nonlinear. 

The  next  sections  explain  how  these  three  steps  are  conducted  for  that  specific 

system. 
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1.  Defining  the  State  and  Output  Variables 

The  angular  rates  determine  how  the  body  behaves  dynamically  in  space.  Suppose 
the  real  values  of  angular  rates  were  known.  Given  an  initial  position,  integrating  the 
angular  rates  through  Euler  equations  would  lead  us  to  determine  the  orientation  at  each 
time  step.  Therefore,  the  angular  rates  can  be  used  to  represent  the  body  dynamics  and 
are  included  in  the  system  state  vector. 

The  angular  rates  are  defined  as 
p:  body  angular  velocity  around  the  x  axis  (roll) 
q:  body  angular  velocity  around  the  y  axis  (pitch) 
r:  body  angular  velocity  around  the  z  axis  (yaw). 

The  second  step  is  to  define  the  outputs  of  the  system.  The  outputs  are  values  that 
must  be  compared  to  observed  values  or  measurements  coming  from  the  sensors.  The 
measurements,  in  this  case,  are  angular  rates,  gravity  vector,  and  local  magnetic  field 
vector  obtained  respectively  from  the  rate  sensors,  accelerometers,  and  magnetometers. 
All  measurements  are  taken  in  body  frame. 

Since  the  angular  rates  are  included  in  state  vector  of  the  system  and  are  also 
outputs,  the  first  three  elements  of  the  output  vector  are  identical  to  those  of  the  state. 

This  simplifies  the  filter  design. 

However,  the  other  six  observations  (in  body  frame)  need  to  be  compared  to  the 
real  values  of  gravity  and  magnetic  field  (in  earth  frame).  This  comparison  is  performed 
after  the  coordinate  transformation,  which  is  based  on  the  orientation  parameters 
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estimated  by  the  filter  (quaternions).  In  other  words,  known  values  of  gravity  and  earth 
magnetic  field  are  obtained  from  gravimetric  and  magnetometric  charts  in  earth 
coordinates.  They  are  converted  into  body  coordinates  using  the  quaternions.  Finally, 
they  are  compared  to  the  observed  values  in  body  coordinates  coming  from  the  sensors. 

The  last  six  elements  of  the  output  vector  are  related  to  constant  values  through 
the  values  of  quaternion  components,  which  are  unknown.  Thus,  the  quaternion 
components  are  included  in  the  state  vector  as  states  to  be  estimated.  The  Extended 
Kalman  Filter  becomes  a  seven-state  filter  in  which  the  states  are  three  angular  rates  and 
four  quaternion  components. 

Having  defined  the  state  and  the  output  vectors,  a  process  model  for  the  state 
variables  can  be  determined. 

2.  The  Process  Model 

The  state  vector  as  defined  in  the  previous  section  has  two  sets  of  variables.  The 
first  set  is  the  three  angular  rates  and  the  second  is  the  four  components  of  the  quaternion. 

The  simplest  process  model  for  angular  rates  is  one  for  generating  those  rates 
from  white  noise.  Figure  3.1  shows  a  diagram  of  this  process  model. 


w. 


Figure  3.1.  Process  Model  for  Angular  Rates. 
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In  the  diagram 

-wr  is  a  3-dimensional  vector  representing  white  noise  sequences  with  known 
covariance  structure  that  generate  p,  q,  and  r ; 

-  Tr  is  the  time  constant  for  p,  q,  and  r ;  and 

-  co  represents  the  angular  rates  p,  q,  and  r. 

The  variances  of  the  white  sequences  and  the  time  constants  were  adjusted  so  that 
the  spectral  characteristics  of  the  signal  generated  by  the  model  match  those  of  the 
angular  rates  under  normal  operational  conditions.  Chapter  IV  will  discuss  how  those 
values  were  chosen. 

The  above  model  represents  the  derivative  of  the  angular  rates  as  a  function  of  the 
angular  rates.  Similar  representation  is  expected  for  the  quaternion  components.  The 
derivative  of  the  quaternions  should  be  a  function  of  the  states  (angular  rates  and/or 
quaternions).  The  next  section  derives  the  dynamics  of  quaternion  components,  so  that 
they  can  fit  in  the  state  space  representation. 

3.  Relating  Angular  Rates  and  Quaternions 

As  it  was  seen  in  the  beginning  of  this  chapter,  the  quaternion  components  were 
included  as  states  to  be  estimated.  Therefore,  a  process  model  for  quaternions  must  be 
defined.  This  means  it  is  necessary  to  find  out  how  the  quaternion  rates  are  related  to  the 
state  (angular  rates  and  qautemion  components).  This  relationship  can  be  obtained  as 
follows  [Ref.  7]. 
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Imagine  three  quaternions  that  perform  the  following  rotations  on  a  vector  v 
:  rotates  a  vector  in  body  frame  into  earth  frame  at  time  t 

m so+Atj  :  rotates  a  vector  in  body  frame  into  earth  frame  at  time  t  +  At 
nB(t+Ao  '•  rotates  a  vector  in  body  frame  at  time  t  +  At  to  body  frame  at  time  t. 
Figure  3.2  shows  the  reference  frame,  the  vector  v  in  two  different  instances  and 
the  quaternions  involved. 


Earth 

Frame 


Figure  3.2.  Determining  the  Quaternion  Rates. 


The  variation  of  the  quaternion  m  in  the  interval  t  +  At  is 


dlTljf  ^B(t+Ai) 


(3.1). 


Using  the  chain  rale 


E0+ Af)  _  _,£(*)  „B(t) 
B(t+At)  ~  rnB(t)  nB(t+A t) 


(3.2). 
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Substituting  Equation  (3.2)  into  (3.1)  leads  to 


The  above  expression  is  divided  by  the  time  interval  and  evaluated  in  the  limit 
when  the  interval  goes  to  zero.  As  u  is  the  direction  of  the  Euler  axis  for  the  rotation 
between  two  positions  in  the  same  frame,  u  is  stationary  in  this  frame.  Thus,  the 
derivative  in  time  of  u  is  the  angular  rate  of  the  body  frame. 


=  2  mB  <°B 


This  equation  indicates  that  the  quaternion  rates  are  a  function  of  the  body  angular 
rates.  Therefore,  they  can  be  included  in  the  process  model  as  if  they  were  generated 
from  the  angular  rates. 


Unlike  the  angular  rates,  the  quaternion  componentes  are  generated  from  a 
mathematical  expression  and  not  from  a  process  driven  by  white  noise.  This  part  of  the 
model  can  be  appended  to  the  first  one  to  complete  the  process  model. 


Using  Equation  3.6,  thejprocess  model  becomes 


Figure  3.3.  Process  Model  for  Angular  Rates  and  Quaternions. 


where 

0  r  -q  p 

-r  0  p  q 
0)  =  „ 

q  ~P  0  r 

-p  -q  -r  0 

is  a  matrix  whose  elements  are  the  angular  rates  and 


(3.7) 


(3.8) 


normalizes  the  quaternion  to  unit  length. 

Since  unit  quaternions  are  needed  to  perform  the  necessary  rotation,  the 
normalization  is  the  constraint  to  be  applied  in  this  case. 

The  states  and  outputs  of  the  system  are  defined  as  well  as  the  process  model.  The 
design  of  the  Kalman  filter  starts  in  the  next  Chapter. 
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IV.  THE  KALMAN  FILTER  DESIGN 


A.  OVERVIEW 

The  Kalman  filter  solves  the  minimum  mean-square  error  (MMSE)  filtering 
problem  using  state  space  methods.  It  has  great  applicability  in  situations  with  nonlinear 
measurement  relationships  [Ref.  9].  The  Extended  Kalman  filter  linearizes  the  equations 
about  a  trajectory  that  is  continually  updated  using  states  estimates. 

This  chapter  starts  presenting  the  state  and  output  equations  used  for  the  Kalman 
filter  design.  Two  approaches  are  discussed.  The  first  approach  works  with  a  set  of 
nonlinear  equations,  but  is  extremely  complicated.  The  second  approach  is  based  on  the 
convergence  properties  of  quaternions.  It  leads  to  another  way  to  solve  the  problem 
without  dealing  with  nonlinear  measurement  equations.  This  approach  is  used  for  the 
design  in  this  thesis. 

B.  FIRST  APPROACH 

As  was  discussed  in  Chapter  III,  the  filter  states  are: 

x, :  angular  rate  p 

x2 :  angular  rate  q 

x3 :  angular  rate  r 

x4  :  quaternion  component  a 

x5 :  quaternion  component  b 
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x6 :  quaternion  component  c 


x1 :  quaternion  component  d  (scalar  component) 

Based  on  the  model  from  Figure  3.3  the  state  equations  can  be  written  as 


1  1 

Xl=-— x,+  — 

^ rx  ^ rx 

(4.1) 

£ 

~  1  ^ 

+ 

4* 

1 

II 

(4.2) 

+ 

1 

II 

•»r 

(4.3) 

*4  =  , - (x,  X,  x,  X,  +  X,  X7  ) 

n  2  ,  2  ,  2,  2  3  3  1  6  17 

2yx4  +  x5  +  x6  +  x7 

(4.4) 

1 

*5=  r—. - 3 - ; - r(  x3  x4 +x,  x6  +  x2  x7) 

2  yx4  +  x5  +  x6  +x7 

(4.5) 

*6  =  r~2 - 2 - 5 - Xlx5+xix1) 

2^x4  +  x5  +  x6  +  x7 

(4.6) 

n - 7 - 2 - T(  ***  *3*«) 

2tJx4  +jc5  +jc6  +x1 

(4.7). 

It  should  be  noted  that  besides  the  non-linearity  in  the  parenthesis  introduced  by 
quaternion  integration,  square-rooted  terms  appear  in  the  denominator  due  to  the 
quaternion  normalization. 

The  outputs  are  the  angular  rates,  and  the  known  values  of  gravity  and  local 
magnetic  field  converted  to  body  coordinates.  They  are  compared  to  the  measurements. 
The  first  three  measurements  come  from  the  rate  gyros.  The  last  six  are  readings  coming 
from  the  accelerometers  (low  frequency  components)  and  from  the  magnetometers. 
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The  outputs  are  defined  as: 

z,  :  angular  rate  p 

z2  •  angular  rate  q 

z3 :  angular  rate  r 

z4  :  component  of  gravity  on  the  x-axis  of  the  body  frame 

z5 :  component  of  gravity  on  the  y-axis  of  the  body  frame 

z6 :  component  of  gravity  on  the  z-axis  of  the  body  frame 

z7  :  component  of  the  local  magnetic  field  on  the  x-axis  of  the  body  frame 

zg :  component  of  the  local  magnetic  field  on  the  y-axis  of  the  body  frame 

z9 :  component  of  the  local  magnetic  field  on  the  z-axis  of  the  body  frame 

The  major  difficult  with  this  approach  is  that  the  coordinate  transformation 
involves  the  computation  of  a  matrix  to  perform  this  transformation.  This  matrix  is  the 
same  as  that  presented  in  equation  2.24,  but  uses  the  values  of  normalized  quaternions. 

'(d2+d2-b2-c2  )  2  (ab-cd)  lidc  +  bd)  1  ,4  g 

R=  liab  +  cd  )  (d2+b2-a2-c2)  2  (bc-ad) 

2  (ac-bd)  2  (bc  +  ad)  (d2+c2-b2-d2\ 

The  filter  design  was  started  using  this  approach  but  the  output  equations  become 
quite  complicated.  As  the  design  of  the  Extended  Kalman  filter  involves  partial 
derivatives,  the  final  expression  seemed  to  be  mathematically  too  complex  to  allow  real¬ 
time  applications. 

Equation  4.9  shows  one  of  the  six  output  equations  for  this  approach 
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(4.9) 


_  (x42  +x72  -x52  -xh2)hx  +  2(x4  x5  -x6x7)h2  +  2(x4x6  +  x5x7)  /z3 

4  ”  ~  o  2  9  o 

X4  +  X$  +  ATg  +  X-j 

where 

ht,h2,  and  h3  are  values  of  earth  magnetic  field  measured  in  earth  coordinates. 

A  second  approach  is  described  in  the  next  section.  It  has  two  key  advantages.  It 
not  only  reduces  the  dimension  of  the  output  vector  but  also  requires  less  computational 
effort. 


C.  ALGORITHMS  FOR  QUATERNION  CONVERGENCE 

1.  Stating  the  Problem 

Imagine  a  body  in  which  a  tri-orthogonal  frame  is  placed  at  its  center  of  gravity.  If 
three  accelerometers  and  three  magnetometers  are  fixed  to  the  origin  of  the  frame,  they 
start  measuring  components  of  the  gravity  and  of  the  earth  magnetic  field  in  the  axis  of 
the  frame.  As  these  values  are  known  and  constant  for  a  limited  geographic  area,  one  can 
expect  that  there  exists  a  quaternion  relating  the  measurements  (values  in  body  frame)  to 
the  real  magnetic  and  gravity  fields  (values  in  earth  frame). 

Obviously  there  are  several  sources  of  errors,  including: 

-  misalignments  between  pairs  of  axes  in  each  sensor; 

-  impossibility  of  placing  both  sensors  at  the  center  of  the  body; 

-  variation  of  both  gravity  and  magnetic  field;  and 

-  errors  inherent  to  the  sensors. 
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That  means  there  is  no  quaternion  that  exactly  converts  what  is  measured  (body 
frame)  into  the  known  values  (earth  frame).  The  solution  is  to  determine  the  best 
quaternion  such  that,  after  the  conversion,  some  criterion  is  satisfied.  This  chapter 
examines  this  problem  using  the  minimum-squared-error  (MSE)  criterion.  The  approach 
is  similar  to  that  applied  in  [Ref.  4],  but  there  a  complementary  filter  was  used.  Another 
difference  is  that  the  error  is  minimized  in  earth  frame  in  this  derivation.  Two  different 
algorithms  are  evaluated  and  some  discussions  about  the  convergence  properties  are 
presented. 

2.  Minimizing  the  Error 

Let  Q  be  the  error  function  defined  as 

EQ  =  eTe  ={Ey,  -  M  \)T  {Eyx  -  M  \) ,  (4. 10) 

where 

:  is  a  6x1  vector  with  values  of  gravity  and  magnetic  field  in  the  earth  frame 
ByQ:  is  a  6x1  vector  with  the  measurements  in  the  body  frame 
and 

where  R  is  the  matrix  defined  by  Equation  2.24. 

Because  y0  and  y,  are  known,  the  error  is  a  function  of  the  matrix  M  ,  and  thus 

it  depends  on  the  four  components  of  the  quaternion.  The  objective  is  to  find  iteratively 
the  values  of  quaternion  components  that  give  the  minimum  error. 
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Several  optimization  algorithms  exist  in  the  literature.  Among  them  the  Gauss  and 
Gauss-Newton  are  the  most  used  ones.  The  main  difference  between  them  is  that  the 
former  uses  the  first  and  second  derivatives  of  the  error  function  (gradient  and  Hessian) 
and  the  latter  uses  only  the  first  derivative  (Jacobian),  which  is  related  to  the  gradient. 

The  different  properties  of  convergence  will  be  addressed  at  the  end  of  this  chapter. 

The  formulation  for  the  iterative  algorithm  can  be  found  in  [Ref.  10].  For  the 
Gauss-Newton  method  it  is  given  as 

nk+ 1  =nk  -[/'(«*) /(«*)]"'  JT{nk)  E£(nk )  (4.11) 


where 

h  is  a  vector  with  the  four  components  of  the  quaternion  and 
J  is  the  Jacobian  matrix  defined  as 


J  =  -\ 


'a*  B  ' 

Jo 


L  V 


da 


rdR  B 


rdR  B  ' 

Yc 


rdR  B 

Yd  y* 


(4.12). 


For  the  Newton  method 


(4.13) 


where 

V  nEQ  is  the  gradient  of  the  error  function  Q  calculated  in  earth  coordinates  with  respect 
to  each  one  of  the  quaternion  components.  The  gradient  is  calculated  using  the  formula 
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In  the  same  way, 
earth  coordinates 
by  the  formula 


V.  EQ  =  - 2 


(dR 

B 

y 

^da 

y0 

A 

(dR 

B 

y 

[db 

a 

(dR 

B 

y 

^ dc 

y0 

(dR 

B 

y 

[ad 

yo 

) 

(4.14). 


the  Hessian  is  the  second  derivative  of  the  error  function  Q  calculated  in 
with  respect  to  each  one  of  the  quaternion  components.  It  is  calculated 


d2R 


d2R 

ddda 


d2R 


dadd 

a2/? 

dd2  . 


\  (Eyi  -RBy0)  - 


'dRB  * T 
\da  .  J 


dRB 
M  y« 


Y 

J 


(dR» 

v3a 


y0 


'a*. 


y  o 


Y 

/ 


(4.15). 


So,  the  main  goal  is  to  find  the  best  values  for  a,  b,  c,  d  such  that  when  the  matrix 
R  is  used  to  convert  the  measurements  to  earth  frame  the  error  is  minimized. 

It  is  worth  noting  that,  as  the  matrix  R  represents  a  coordinate  transformation,  it 
is  orthonormal.  Thus,  the  only  difference  when  minimizing  the  error  in  body  coordinates 
is  that  the  transpose  of  R  is  applied  to  the  known  values  in  earth  frame.  This  leads  to 
similar  expressions  and  does  not  affect  the  convergence  properties. 
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D.  SECOND  APPROACH 


With  the  introduction  of  the  convergence  algorithm  as  an  external  loop  to  the 
Kalman  filter,  the  quaternion  components  are  now  computed  measurements.  No 
equations  involving  coordinates  transformations  need  to  be  evaluated.  The  algorithm  is 
initialized  using  the  last  value  of  quaternion,  which,  for  small  step  sizes  is  very  close  to 
the  next  value.  Figure  4.5  shows  how  simple  the  filter  becomes  and  gives  a  general  idea 
of  the  data  flow. 

Once  again,  it  should  be  noted  that  regardless  of  where  the  error  is  minimized 
(body  or  earth  coordinates),  the  convergence  is  not  affected.  However,  it  is  important  that 
the  Kalman  filter  knows  which  one  is  being  used. 


Figure  4.5.  Final  Diagram  for  the  Filter. 
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For  the  final  system  the  inputs  are  the  same  as  before.  However,  the  new  outputs 
are 

Zj  :  angular  rate  p 

z2  •  angular  rate  q 

z3 :  angular  rate  r 

z4  :  quaternion  component  a 

z5 :  quaternion  component  b 

z6 :  quaternion  component  c 

z7  :  quaternion  component  d. 

As  can  be  seen  the  outputs  are  identical  to  the  states.  They  are  no  longer  related  to 
the  states  by  the  nonlinear  equations. 
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V.  MODELING  SYSTEM  NOISE 


A.  INTRODUCTION 

One  important  assumption  in  the  Kalman  Filter  is  that  any  errors  in  the  control 
input  vector  and  the  measurement  vector  are  zero-mean  and  follow  a  Gaussian 
distribution.  The  Kalman  Filter  uses  those  two  vectors  as  well  as  their  covariance 
matrices.  They  are  determined  based  on  the  motion  dynamics  of  the  body  or  vehicle 
(input  vector)  and  on  the  characteristics  of  the  sensors  used  (measurement  noise).  In  order 
for  the  filter  to  achieve  its  best  performance,  the  statistical  properties  of  those  vectors 
must  be  determined 

It  happened  that,  at  the  time  this  thesis  was  written,  neither  MARG  sensors  nor 
data  from  rigid  body  experiments  were  available.  The  only  data  set  available  was  from 
experiments  with  the  Small  Autonomous  Underwater  Vehicle  Navigation  System 
(SANS)  [Ref.  1 1].  That  set  includes  readings  of  angular  rates  and  gravity  for  one 
experiment  and  was  used  as  basis  for  determining  the  statistical  characteristics  of  the 
process  to  be  estimated  and  of  the  sensors  utilized.  The’ following  sections  describe  the 
procedures  applied  to  obtain  the  statistical  properties  of  both  the  input  and  measurement 
vectors. 
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B.  PROCESS  NOISE  AND  TIME  CONSTANTS 


The  process  noise  wk  is  the  noise  that  when  applied  as  an  input  to  the  process 

model  gives  the  desired  process  as  the  output.  Figure  3.2  is  repeated  here.  It  represents 
the  process  model  for  the  angular  rates  and  quaternions. 


Figure  3.2.  Process  Model  for  Angular  Rates  and  Quaternions. 

The  goal  is  to  determine  the  values  of  noise  variance  and  time  constants  that 
generate  values  of  p,  q,  and  r  matching  with  real  ones.  It  was  assumed  that  the  operation 
conditions  of  the  vehicle  were  known  (in  this  case  the  underwater  vehicle).  Values  of 
angular  rates  from  the  SANS  test  were  used.  The  process  model  was  simulated  with 
Simulink.  Figure  5.1  shows  the  corresponding  diagram.  The  S-function  is  a  Matlab 
ODE45  routine  that  integrates  Equations  4. 1  through  4.7. 
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Num  ber 


Figure  5.1.  Simulation  of  the  Process  Model  for  p,  q,  and  r. 


The  values  of  noise  variance  and  the  time  constants  were  adjusted  until  the  best 
match  was  obtained.  Table  5.1  presents  the  best  values  for  the  parameters. 


Variance  (rad2/sec2) 

Time  constant  (sec) 

p 

lxlOW 

0.002 

q 

1X10-4 

0.15 

r 

9x1  O'2 

1.0 

Table  5.1.  Values  of  Variance  and  Time  Constant  for  the  Process  Model. 


The  results  of  the  simulation  using  the  Simulink  and  the  values  of  Table  5.1  were 
compared  to  the  real  values  of  angular  rates.  As  the  model  is  a  simple  first  order  model, 
the  results  can  be  considered  satisfactory.  Figure  5.2  shows  a  comparison  between  the 
real  and  the  simulated  processes. 
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xio  4  Real  Values  of  Angular  Rates  x  iQ4values  Generated  bythe  Model 


0  10  20  30  40  50  0  10  20  30  40  50 


time  (sec)  time  (sec) 

Figure  5.2.  Comparison  Between  Real  and  Modeled  Processes. 

Two  facts  should  be  mentioned.  First,  the  process  model  as  depicted  in  Figure  3.2 
does  not  have  noise  generating  quaternions.  Therefore  only  three  variances  are  needed  to 
describe  the  statistics  of  the  process.  Despite  that,  one  has  to  keep  in  mind  that  the 
Equations  4.1  through  4.7  rule  the  entire  process.  So  slight  changes  on  the  values  of 
variance  or  time  constant  of  the  variable  p  can  affect  the  processes  of  the  other  six 
variables. 
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Second,  there  is  no  means  of  checking  the  values  of  the  quaternions  generated  by 
the  model.  However,  as  that  part  of  the  model  involves  only  mathematical  equations  of 
deterministiv  nature,  it  is  expected  that  the  model  fits  reality. 

Based  on  the  above  assumptions,  the  covariance  matrix  of  the  process  noise  is 
defined  as 


"lxlO-14 

0 

0 

0 

0 

0 

o' 

0 

lxlO-4 

0 

0 

0 

0 

0 

0 

0 

9x1  (T2 

0 

0 

0 

0 

<2  = 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

0 

Note  that  the  cross  correlation  terms  are 

all  zero. 

(5.1). 


C.  MEASUREMENT  NOISE 

The  values  of  measurement  noise  for  the  rate  sensors  and  accelerometers  were 
determined  from  the  same  data  set.  Small  intervals  in  which  the  values  of  the  angular  rate 
and  gravity  components  seem  to  be  stationary  were  used  to  estimate  the  values  of 
variances.  Table  4.2  presents  the  values  obtained. 

The  values  above  can  be  compared  to  those  from  [Ref.  3]  and  [Ref.  8],  They  both 
use  MotionPak  inertial  measurement  units  made  by  Systran  Donner,  which  include 
Sundstrand  Q-Flex  accelerometers  and  Systran  Donner  GyroChips.  The  values  used  in 
those  references  are  presented  in  Table  5.3.  As  can  be  seen  the  estimated  values  from 


Table  5.2  are  quite  reasonable  and  thus,  they  are  be  used  for  noise  variances  of  the 
angular  rate  sensors  and  accelerometers. 


Variance 

p 

5.4  x  10"9  (rad/sec)2 

q 

2.2  x  10'4  (rad/sec)2 

r 

8.4  x  lO'4  (rad/sec)2 

gx 

■ 

gy 

0.028  (m/sec2)2 

gz 

Table  5.2.  Values  of  Variance  for  the  Measurement  Noise. 


Angular  rates 

Acceleration 

[Ref.  8] 

[Ref.  9] 

6.4  x  10'3  (rad/sec)2 

3  x  10'5  (m/sec2)2 

Table  5.3.  Values  of  Variances  for  Angular  Rates  and  Acceleration. 


The  data  set  does  not  include  measurements  of  the  earth’s  magnetic  field. 
Therefore,  no  estimate  can  be  made.  However,  [Ref.  9]  mentions  the  variance  for  a  two- 
axis  magnetometer  made  by  Precision  Navigation  Inc.  as  2  degrees.  That  value 
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corresponds  to  a  variance  of  approximately  10'3  if  one  considers  a  unitary  normalized 
value  for  earth  magnetic  field. 

Given  the  above  discussion,  the  measurement  noise  covariance  matrix  can  be 
written  as 

5.4xl0-9  0  0  0  0  0  0  0  0  ~ 

0  2.2xl0~4  0  0  0  0  0  0  0 

0  0  8.4xl0"4  0  0  0  0  0  0 

0  0  0  0.012  0  0  0  0  0 

<2  =  0  0  0  0  0.028  0  0  0  0  (5.2) 

0  0  0  0  0  0.002  0  0  0 

0  0  0  0  0  0  0.001  0  0 

0  0  0  0  0  0  0  0.001  0 

0  0  0  0  0  0  0  0  0.001_ 

Note  that  all  cross  correlation  terms  in  Q  are  also  zero. 

Another  important  observation  is  related  to  the  dimension  of  Q.  The  matrix  is  9  x 
9  because  it  reflects  the  noises  of  nine  sensors  that  furnish  the  measurements.  However, 
the  state  space  was  defined  as  having  six  states  and  seven  outputs.  None  of  those 
accelerations  or  magnetic  fields  appear  directly  in  the  measurements.  As  it  was  discussed, 
the  measurements  are  converted  through  the  convergence  algorithm  into  quaternions 
measurements.  That  conversion  transforms  the  9  x  9  <2  matrix  into  a  new  7  x  7  <2  matrix 
explained  in  the  next  section. 
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D.  COVARIANCE  MATRIX  OF  THE  MEASUREMENT 

From  the  convergence  algorithm  (Equation  4.1 1),  the  following  relationship  is 
determined  between  the  measurement,  which  has  known  covariance  matrix,  and  the 
quaternion  components: 


»*+i  =  "*  - F(nk )[Ey- R{nt )  fiy] 

(5.3) 

=nk~  Fink )  Ey  +  F{nk )  R{nk )  By 

(5.4). 

That  means  the  values  for  the  quaternions  that  the  convergence  algorithm 

calculates  are  a  function  of  the  previous  value  or  the  guess.  F(nk  )  depends  on  which 

type  of  algorithm  is  being  used  (Newton  or  Gauss-Newton). 

The  above  expression  can  be  rewritten  as 

y  =  Ax+B,  (5.5) 

where 

B  =  nk-F{nk)Ey  (5.6) 

and 

A  =  F(njR(nJ  (5.7). 

Assume  that  after  one  step  the  optimization  algorithm  converges  to  a  reasonable 
value  and  that  the  initial  guess  always  has  the  same  value.  Then,  the  parameters  A  and  B 
are  constant  for  any  iteration  of  the  Kaman  filter.  Using  the  same  procedure  as  [Ref.  9], 
the  covariance  matrix  for  the  quaternion  components  is  given  by 

C„=ACyAr  (5.8). 
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Two  observations  should  be  made.  If  the  initial  guess  is  not  the  same,  the  values 
of  A  and  B  change  at  each  of  the  Kalman  filter  iteration.  This  is  not  a  problem  because  it 
is  easy  to  implement  a  covariance  matrix  that  depends  on  the  guess.  Second,  even  if  one 
considers  that  the  convergence  takes  more  than  one  iteration,  the  values  of  A  and  B  are 
still  functions  of  the  initial  guess. 

In  this  thesis  the  first  option  is  used  and  the  covariance  matrix  is  updated  on  each 
step  with  the  value  of  a  =  F{nk )  R(nk)  coming  from  the  convergence  algorithm. 
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VI.  TESTS  AND  SIMULATIONS 


A.  INTRODUCTION 

The  Extended  Kalman  filter  design  based  on  the  second  approach  achieves  good 
results  provided  the  algorithm  that  calculates  the  computed  quaternion  converges  in  few 
steps. 

This  chapter  begins  describing  how  the  Kalman  filter  algorithm  was  tested  and 
presents  some  results  of  the  convergence  process.  It  also  presents  all  static  and  dynamic 
tests  performed  on  the  complete  filter. 

As  no  MARG  sensor  was  available  at  the  time  this  thesis  was  being  completed,  all 
data  used  here  is  generated  mathematically. 

B.  TESTS  OF  THE  CONVERGENCE  ALGORITHM 

The  objective  of  the  test  is  to  check  the  convergence  for  different  rotations,  initial 
estimates,  and  noise  levels.  A  six-element  vector  is  chosen,  which  contains  the 
components  of  gravity  and  local  magnetic  field  vectors.  An  arbitrary  quaternion  is 
selected  and  used  to  rotate  the  initial  vector.  Gaussian  noise  is  added  to  the  rotated  vector 
in  order  to  simulate  the  measurement  noise.  The  initial  guesses  are  always  values  around 
the  real  value  of  the  quaternion.  The  following  examples  illustrate  the  procedure. 

Suppose  the  following  values  are  measured  in  body  frame: 

®y  =  [l  3  5  2  -4  0] 
and  the  values  in  earth  frame  as  known  as 
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£y  =  [  5  1  3  0  2  -4]. 

The  initial  guess  is  taken  to  be  the  quaternion 
n  =  [  0.3  0.4  0.6  0.7  ], 
which  after  normalization  is 

n  =  [0.2020  0.3030  0.6061  0.7071  ]. 

Figure  6. 1  shows  the  convergence  when  using  the  Gauss-Newton  method.  As  can 
be  seen,  the  algorithm  converges  in  only  three  iterations.  Figure  6.2  shows  the 
convergence  for  the  Newton  algorithm.  In  both  cases,  no  noise  was  added. 


Quaternions  Convergence 


Figure  6.1.  Quaternion  Convergence  Using  Gauss-Newton  (no  Noise). 
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Quaternions  Convergence 


Figure  6.2.  Quaternion  Convergence  Using  Newton  (no  Noise). 

In  the  experiments  above,  the  quaternion  converges  to  h  =  [  0.5  0.5  0.5  0.5]. 
As  it  was  shown  in  Chapter  II,  this  quaternion  corresponds  to  a  rotation  of  120 
degrees  about  the  axis  [  1  1  1  ] .  As  there  is  no  measurement  noise,  the  values  are  the 
same  in  both  frames  but  with  the  order  changed.  The  error  in  this  case  is  zero. 

Suppose  noise  is  added  to  the  measurement  such  that 
By  =  [\2  3.1  4.8  1.9  -3.7  0.3] 

The  algorithm  now  converges  to  the  quaternion 
n  =  [  0.5291  0.5127  0.4912  0.4989]. 
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This  is  the  best  value  that  can  be  found  and  the  error  is  different  from  zero.  Figure 


6.3  shows  the  convergence  using  the  Gauss-Newton  algorithm. 

Quaternions  Convergence 


Figure  6.3.  Quaternion  Convergence  Using  Gauss-Newton  (with  Measurement 

Noise). 

Extensive  simulation  was  carried  out  with  the  Matlab  routine  from  Appendix  A. 
In  this  routine  two  unit  vectors  represent  gravity  and  local  magnetic  field.  The  following 
elements  are  set  in  the  routine: 

a  vector  representing  the  axis  of  rotation 

-  the  variance  of  the  measurement  noise 

-  distance  between  the  real  value  of  quaternion  and  initial  estimates. 

The  program  rotates  the  unit  vectors  about  the  axis  of  rotation  through  several 
angles,  adds  noise,  and  solves  the  convergence  problem  for  each  case. 
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Table  6.1  shows  how  the  values  converge  after  three  iterations  of  the  Gauss- 
Newton  algorithm.  The  vectors  are  rotated  about  the  axis  m  =  [  1  3  2  ]  through  different 
angles  and  the  noise  measurement  has  standard  deviation  0.05. 


Angle 

a 

a_hat 

b 

b_hat 

c 

c_hat 

d 

d_hat 

20 

0.0464 

0.0465 

0.1392 

0.1379 

0.0928 

0.0939 

0.9848 

0.9849 

40 

0.0914 

0.0905 

0.2742 

0.2765 

0.1828 

0.1816 

0.9397 

0.9394 

60 

0.1336 

0.1347 

0.4009 

0.3992 

0.2673 

0.2680 

0.8660 

0.8664 

80 

0.1718 

0.1676 

0.5154 

0.5205 

0.3436 

0.3413 

0.7660 

0.7645 

0.2047 

0.2046 

0.6142 

0.6143 

0.4095 

0.4094 

0.6428 

0.6428 

120 

0.2315 

0.2311 

0.6944 

0.6946 

0.4629 

0.4628 

0.5000 

0.4999 

140 

0.2511 

0.2511 

0.7534 

0.7534 

0.5023 

0.5023 

0.3420 

0.3420 

160 

0.2632 

0.2605 

0.7896 

0.7911 

0.5264 

0.5256 

0.1736 

0.1734 

180 

0.2673 

0.2768 

0.8018 

0.7970 

0.5345 

0.5369 

0.0000 

0.0000 

0.2632 

0.7896 

0.7976 

0.5264 

0.5232 

-0.1736 

-0.1721 

220 

0.2511 

0.2548 

0.7534 

0.7517 

0.5023 

0.5027 

-0.3420 

-0.3426 

240 

0.2315 

0.2384 

0.6944 

0.6909 

0.4629 

0.4630 

-0.5000 

-0.5015 

260 

0.2047 

0.2097 

0.6142 

0.6115 

0.4095 

0.4090 

-0.6428 

-0.6440 

280 

0.1718 

0.1751 

0.5154 

0.5135 

0.3436 

0.3428 

-0.7660 

-0.7669 

300 

0.1336 

0.1338 

0.4009 

0.4008 

0.2673 

0.2672 

-0.8660 

-0.8661 

320 

0.0914 

0.2742 

0.2732 

0.1828 

0.1815 

-0.9397 

-0.9400 

340 

0.0464 

0.0476 

0.1392 

0.1390 

0.0928 

0.0917 

-0.9848 

-0.9849 

360 

0.0000 

-0.0003 

0.0000 

0.0005 

-1.0000 

-1.0000 

Table  6.1.  Experiment  to  Verify  the  Convergence  Using  Gauss-Newton  Method. 


C.  STATIC  TESTS  OF  THE  COMPLETE  FILTER 

The  purpose  of  the  static  tests  is  to  verify  that  the  filter  converges  to  the  correct 
steady-state  values  after  a  single  rotation.  The  body  frame  of  the  sensors  is  at  rest,  but 
with  a  fixed  orientation  relative  to  the  earth  frame.  Among  the  ten  different  tests 
performed,  three  are  presented  in  this  thesis  as  described  below. 
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1.  Static  Test  1  -  Two  Coincident  Frames 

Figure  6.4  shows  the  position  of  the  vectors  involved  in  both  body  and  earth 

frames. 


Earth  Frame  Body  Frame 


Figure  6.4.  Static  Test  1-  Two  Coincident  Frames. 


Unitary  values  of  g  (gravity)  and  h  (magnetic  induction)  were  used.  The 
measurements  were  generated  as 

fig=(0,0,-l)  and  Bh  =  {-  0.5,0,0.866). 

The  measurement  noises  specified  in  Chapter  V  were  added  to  those  values  that 
represent  the  sensor  readings  in  the  body  frame. 

Table  6.2  shows  the  initial  values  utilized  for  the  states  and  the  expected  steady- 
state  values.  Figures  6.5  and  6.6  present  the  simulation  results. 


54 


O 

a 

n 

a 

D 

D 

m 

Initial  Values 

1.0 

1.0 

1.0 

0.5 

0.5 

0.5 

0.5 

Steady-state  Values 

0 

0 

0 

0 

0 

0 

1.0 

Table  6.2.  Static  Test  1  -  Initial  and  Steady-state  Values. 


Covergence  of  Angular  Rates 


time  (sec) 


Figure  6.5.  Static  Test  1  -  Convergence  of  Angular  Rates. 
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Figure  6.6.  Static  Testl  -  Convergence  of  Quaternion  Components. 


2.  Static  Test  2  -  Frames  Related  by  the  Quaternion  (0.5 , 0.5 , 0.5 , 0.5) 

Figure  6.7  shows  the  position  of  the  vectors  involved  in  both  frames.  This  case 
corresponds  to  that  discussed  in  Chapter  n.  Both  frames  are  coincident  and  then  the  body 
frame  is  rotated  1 20  degrees  about  the  axis  ih  =  [  1  1  1  ] . 
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Earth  Frame  Body  Frame 

Figure  6.7.  Static  Test  2  -  Rotated  Frame. 


Table  6.3  shows  the  initial  values  utilized  for  the  states  and  the  expected  steady- 
state  values.  Figures  6.8  and  6.9  present  the  simulation  results. 


D 

a 

a 

a 

n 

a 

d 

Initial  Values 

1.0 

1.0 

1.0 

1.0 

1.0 

1.0 

1.0 

Steady-state  Values 

0 

0 

0 

0.5 

0.5 

0.5 

0.5 

Table  6.3.  Static  Test  2  -  Initial  and  Steady-state  Values. 
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yaw  (rad/sec)  pitch  (rad/sec)  roll  (rad/sec) 


Covergence  of  Angular  Rates 


0  0.5  1  1.5  2  2.5  3 

time  (sec) 

Figure  6.8.  Static  Test  2  -  Convergence  of  Angular  Rates. 


Covergence  of  Quaternion  Components  -  ai  +  bj  +  ck  +  d 


0  0.5  1  1.5  2  2.5  3 


time  (sec) 


Figure  6.9.  Static  Test  2  -  Convergence  of  Quaternion  Components. 
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3.  Static  Test  3  -  Frames  Related  by  the  Quaternion  (0 , 0.707 , 0 , 0.707) 

Figure  6.10  shows  the  position  of  the  vectors  involved  in  the  measurements  in 
both  frames.  This  case  corresponds  to  rotating  the  body  frame  about  the  y-axis  through  an 
angle  of  90  degrees. 


z 


Earth  Frame  Body  Frame 

Figure  6.10.  Static  Test  3-  Frame  Rotated  About  One  of  Its  Axis. 


Table  6.4  shows  the  initial  values  utilized  for  the  states  and  the  expected  steady- 
state  values.  Figures  6.1 1  and  6.12  present  the  simulation  results. 


a 

D 

n 

a 

u 

C 

ma 

Initial  Values 

1.0 

1.0 

1.0 

1.0 

1.0 

1.0 

1.0 

Steady-state  Values 

0 

0 

0 

0 

0.71 

0 

0.71 

Table  6.4.  Static  Test  3  -  Initial  and  Steady-state  Values. 
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1 


Covergence  of  Angular  Rates 


time  (sec) 


Figure  6.11.  Static  Test  3  -  Convergence  of  Angular  Rates 


Covergence  of  Quaternion  Components  -  ai  +  bj  +  ck  +  d 


0  0.5  1  1.5  2  2.5  3 

time  (sec) 

Figure  6.12.  Static  Test  3  -  Convergence  of  Quaternion  Components. 
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B.  DYNAMIC  TESTS  OF  THE  COMPLETE  FILTER 


The  dynamic  tests  intend  to  verify  if  the  filter  correctly  estimates  the  attitude 
parameters  when  the  body  is  moving. 

Unlike  the  convergence  and  static  tests,  where  the  data  is  easily  generated  by  one 
rotation  of  the  values  of  gravity  and  local  magnetic  field  vectors,  in  the  dynamic  tests  the 
attitude  is  changing  continuously.  In  this  case  a  procedure  to  generate  the  data  is  more 
complex  and  involves  computing,  at  each  step,  the  body  orientation.  The  procedure  is 
described  as  follows. 

1.  Generating  Data 

Suppose  body  and  earth  frame  are  coincident  such  that  the  initial  attitude 
parameters  (Euler  angles  or  quaternions)  are  known.  If  a  rotation  is  applied  to  the  body, 
one  can  easily  determine  how  the  parameters  evolve  in  time  by  integrating  Equation  3.6 
or  the  Euler  equations  [Ref.  12].  In  the  first  case,  the  quaternion  components  are  obtained 
directly  whereas  in  the  second  Euler  angles  need  to  be  converted  into  direct  cosine  matrix 
and  then  quaternions. 

As  the  goal  is  to  check  the  quaternion  filter  output,  the  first  option  would  not 
work  with  quaternion  during  the  data  generation.  Therefore,  the  process  of  generating 
data  for  the  experiment  has  four  steps.  An  angular  rate  profile  is  defined.  The  integration 
of  that  profile  using  Euler  equations  leads  to  the  Euler  angles.  The  initial  conditions  are 
always  null  Euler  angles  corresponding  to  the  position  with  both  frames  aligned.  Those 
angles  are  used  to  make  up  the  rotation  matrix  that  transforms  value  of  gravity  and  local 
magnetic  field  vectors  from  earth  frame  to  body  frame.  Gaussian  noise  is  added 
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representing  the  measurement  noise.  Figure  6.13  shows  the  diagram  of  the  process. 
Appendix  C  includes  the  code  for  the  process. 


Angular 
Rates  Profile 
P,  q.  r 


Gravity  and 
Earth  Magnetic 

Field  Gaussian 


Simulated 

Data 


Figure  6.13.  Data  Generation  for  Dynamic  Tests. 

Two  dynamic  tests  are  presented  in  this  thesis.  The  angular  rates  are  always 
applied  as  pulses  that  rotate  the  body  exactly  90  degrees.  Thus,  it  is  easy  to  check  if  the 
filter  converges  to  the  right  values. 

2.  Dynamic  Test  1  -  Gaussian  Pulse  and  Rotation  About  One  Axis 

For  this  test  only  rotation  about  the  x-axis  is  performed.  The  profile  for  that 
rotation  is  a  Gaussian  pulse  as  seen  in  Figure  6. 14.  When  integrated  by  Euler  equations 
the  final  position  of  the  body  frame  corresponds  to  a  90-degree  rotation  about  that  axis. 
Figure  6. 15  shows  both  frames  after  the  rotation  is  finished. 
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angular  rates  (rad/sec) 


Values  of  Angular  Rates 


Figure  6.14.  Dynamic  Test  1  -  Angular  Rates  Profile. 


Earth  Frame  Body  Frame 


Figure  6.15.  Dynamic  Testl  -  Final  Position  After  Rotation. 


The  quaternion  relating  these  two  positions  can  be  calculated  by  applying  a 
rotation  of  90  degrees  about  the  x-axis.  The  value  of  that  quaternion  is 

(Jo  Jo  \ 

n  =  (  sin  45°  ( 1 , 0 , 0  ) ,  cos  45°)  =  — ,0,0,—  =(  0.71 , 0 , 0 , 0.71  ). 

V  J 

Figure  6.16  shows  the  estimates  for  of  angular  rates.  The  Gaussian  pulse  is 
correctly  estimated  as  well  as  the  absence  of  rotation  about  the  y  and  z-axes. 


Covergence  of  Angular  Rates 


time  (sec) 


Figure  6.16.  Dynamic  Test  1  -  Convergence  of  Angular  Rates. 

Figure  6.17  shows  the  convergence  of  the  quaternion  components.  At  the 
beginning,  they  converge  almost  immediately  to  the  initial  values  (both  frames 
coincident).  The  final  values  are  exactly  the  ones  calculated  above. 
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C overgen ce  of  Quaternion  Components  -  ai  +  bj  +  ck  +  d 


time  (sec) 


Figure  6.17.  Dynamic  Test  1  -  Convergence  of  Quaternion  Components. 


3.  Dynamic  Test  2  -  Square  Pulse  and  Two  Consecutive  rotations 

In  this  second  test  two  rotations  are  performed.  The  profile  for  those  rotations  can 
be  seen  in  Figure  6.18.  The  two  square  pulses  rotate  the  body  consecutively  90  degrees 
about  x-axis  and  90  degrees  about  y-axis.  In  this  case,  the  final  position  after  the  first 
rotation  is  the  same  of  the  dynamic  test  1  and  the  compound  rotation  is  equivalent  to  a 
single  rotation  through  120  degrees  about  the  axis  in  =  [  1  1  1  ],  which  is  the  same  case 
of  the  static  test  2.  Therefore,  the  final  quaternion  realting  both  frames  is 

n  =  (  0.5 , 0.5 , 0.5 , 0.5  ). 

Figure  6.19  shows  both  frames  after  the  rotations  are  complete. 
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x  g  z  g 

Earth  Frame  Body  Frame 


Figure  6.19.  Dynamic  Test  2  -  Final  Position  After  Rotations. 
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As  expected,  the  filter  estimates  the  angular  rates  and  the  quaternion  components 
even  in  the  pulse  transitions.  Figures  6.20  and  6.21  present  the  results  of  the  estimates. 


Covergence  of  Angular  Rates 


time  (sec) 


Figure  6.20.  Dynamic  Test  2  -  Convergence  of  Angular  Rates. 


Covergence  of  Quaternion  Components  -  ai  +  bj  +  ck  +  d 


0  50  100  150  200  250  300 


time  (sec) 

Figure  6.21.  Dynamic  Test  2  -  Convergence  of  Quaternion  Components. 
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VII.  CONCLUSIONS 


A.  SUMMARY 

This  thesis  presented  a  complete  design  of  an  Extended  Kalman  filter  for  real¬ 
time  estimation  of  rigid  body  motion  attitude.  The  use  of  quaternions  to  represent 
rotations,  instead  of  Euler  angles,  eliminated  the  long-standing  problem  of  singularities 
called  “gimbal  lock”  defined  in  Chapter  n. 

A  simple  first-order  process  model  for  rigid  body  angular  rate  measurements  was 
defined,  which  closely  matched  real  process  data  obtained  from  angular  rate  sensors. 

The  quaternion  rates  were  nonlinearly  related  to  the  current  quaternion  and  the  angular 
rates.  The  process  model  converted  angular  rates  into  quaternion  rates,  which  were 
integrated  to  obtain  quaternions. 

Two  approaches  to  the  Kalman  filter  design  were  investigated.  The  first  approach 
used  nine  output  equations:  three  angular  rates,  three  components  of  linear  acceleration, 
and  three  components  of  the  earth  magnetic  field.  Since  these  output  equations  were 
highly  nonlinear  functions  of  the  process  state  variables,  the  partial  derivatives  needed 
for  the  Kalman  filter  design  were  very  complicated.  As  a  result,  it  was  decided  that  a 
filter  formulated  with  these  equations  would  not  be  useful  for  real-time  applications. 

The  second  approach  utilized  Gauss-Newton  iteration  to  find  the  optimal 
quaternion  that  related  the  values  of  linear  accelerations  and  earth  magnetic  field  in  the 
body  coordinate  frame  and  in  the  earth  coordinate  frame.  The  optimal  quaternion  was 
used  as  part  of  the  measurement  for  the  Kalman  filter,  which  had  seven  output  equations: 
three  angular  rates  and  four  components  of  quaternions.  Furthermore,  the  output 
equations  were  linear  and  the  output  matrix  was  the  identity  matrix.  As  a  result,  the 
partial  derivatives  were  total  derivatives  and  had  constant  values.  The  convergence 
algorithm  replaced  the  computation  of  the  partial  derivatives  of  the  nine  nonlinear 
measurement  equations.  The  computational  requirements  for  the  Kalman  filter  developed 
using  this  approach  were  significantly  reduced,  making  it  possible  to  estimate  attitude  in 
real  time. 
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B.  ANALYSIS  OF  RESULTS 


Extensive  tests  were  conducted  to  verify  the  convergence  of  Gauss-Newton  and 
Newton  algorithms,  and  the  performance  of  the  Kalman  filter.  In  almost  all  cases  and  for 
both  Gauss-Newton  and  Newton  algorithms,  the  convergence  to  acceptable  values 
occurred  in  less  than  three  iterations.  In  very  few  cases  (about  0.5%)  where  the  algorithm 
diverged,  the  Kalman  filter  did  not  use  the  quaternion  information.  The  filter 
implementation  used  only  the  Gauss-Newton  algorithm  because  it  does  not  involve 
second  partial  derivatives. 

The  filter  achieved  excellent  results  for  all  static  and  dynamic  tests.  The 
convergence  to  steady-state  values  took  only  two  or  three  iteration  steps.  The  two 
examples  presented  in  this  thesis  showed  that  the  filter  was  able  to  track  angular  rates 
either  for  smooth  inputs  (Gaussian  pulse)  or  for  abrupt  ones  (square  pulses).  Similar 
results  were  observed  for  the  quaternion  estimates.  They  quickly  converged  to  the  correct 
values  and  tracked  the  orientation  of  the  body  frame.  No  singularities  were  noticed,  even 
when  the  two  consecutive  rotations  described  in  Chapter  II  were  applied  (static  test  2  and 
dynamic  test  2). 


C.  SUGGESTIONS  FOR  FUTURE  WORK 

Although  the  current  filter  worked  well,  there  is  much  work  that  could  be  done  to 
further  improve  its  performance.  A  few  possibilities  are  as  follows. 

First,  one  can  adjust  the  statistic  parameters  of  the  process  model  such  that  the 
filter  design  corresponds  to  the  process  to  be  estimated  and  sensors  to  be  used. 

Second,  to  make  this  filter  faster,  further  work  needs  to  be  carried  out  on  the 
convergence  algorithm.  The  fact  that  it  optimizes  a  function  that  includes  a  block 
diagonal  matrix  can  be  used  to  simplify  the  algorithm,  mainly  the  steps  involving  matrix 
inversion. 

Third,  the  filter  should  be  tested  using  real  data  obtained  from  the  MARG  sensors 
currently  under  development  at  the  NPS. 
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Finally,  the  best  test  would  be  the  application  of  the  attitude  estimation  into  a 
filter  that  estimates  position  of  a  body  or  vehicle  in  real-time. 


THIS  PAGE  INTENTIONALLY  LEFT  BLANK 
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APPENDIX  A  -  ROUTINES  FOR  CONVERGENCE  TESTING 


Al.  GAUSS-NEWTON  METHOD 

% 

%  Gauss -Newton  Method  -  Normalized  Guess 
% 

%  Evaluate  the  convergence  for  angles  of  rotation  between  10  and  350  % 
%  degrees 
% 

%  By  Joao  Luis  Marins,  Monterey,  July  2000. 


clc;  clear; 

yb  =  input ('Enter  the  a  six-element  vector  in  body  coordinates  :  1 ); 

h  =  input ('Enter  the  axis  of  rotation  (three-element  row  vector)  :  '); 

res  =  [ ] ; 

for  theta  =  10:20:350 

theta  =  theta*pi/180 ; 

%  Define  the  measurement  noise 
var  =  0; 

noise  =  sqrt (var ) *randn ( 6 , 1) ; 

%  Calculate  the  quaternion  for  the  angle  and  axis  chosen 
h  =  h/ ( sqrt (h*h ' ) ) ; 

n  =  [  ( (sin(theta/2) ) *h) 1  ;  cos (theta/2) ]* ; 

%  Rotate  the  known  vector  and  add  measurement  noise 
a=n { 1 ) ;  b=n(2) ;  c=n(3);  d=n(4) ; 

M  =  [dA2+aA2-b^2-c^2  2*(a*b-c*d)  2*(a*c+b*d); 

2  * (a*b+c*d)  dA2+bA2-cA2-aA2  2* (b*c-a*d) ; 

2  * (a*c-b*d)  2* (b*c+a*d)  dA2+cA2-aA2-bA2 ] ; 

R  =  [M  zeros (3)  ;  zeros (3)  M] ; 
ye  =  (R*yb ' ) ; 
ye  =  ye  +  noise; 

%  Distance  between  correct  value  and  guess 
njiat  =  (n  -  [0.02  0.02  0.02  0.02])'; 

ac_n  = [ ] ; 

%  Three  iterations  using  the  normalized  quaternion 
for  i=l : 3 

n„hat  =  n_hat/ (sqrt (n_hat ' *n_hat) ) ; 
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if  i==l 

n_guess  =  n_hat; 

end 

ac_n  =  [ac_n  n_hat]  ; 

a=n_hat ( 1 ) ;  b=n_hat ( 2 ) ;  c=n_hat ( 3 ) ;  d=n_hat ( 4 )  ; 

%  Calculate  the  partial  derivatives  needed  for  the  Jacobian 
R_a  =  t  a  b  c  ;  b  -a  -d  ;  c  d  -a] ; 

d_R_a  =  2*[R_a  zeros (3)  ;  zeros (3)  R_a] ; 

R_b  =  [-b  a  d  ;  a  b  c  ;  -d  c  -b] ; 

d_R__b  =  2*[R_b  zeros  (3)  ;  zeros  (3)  R_b]  ; 

R-_c  =  [  — c  -d  a  ;  d  -c  b  ;  a  b  c]  ; 

d__R_c  =  2*[R__c  zeros  (3)  ;  zeros  (3)  R_c]  ; 

R_d  =  [  d  -c  b  ;  c  d  -a  ;  -b  a  d]  ; 

d_R_d  =  2*[R_d  zeros (3)  ;  zeros (3)  R_d] ; 

jacobian  =  -[  (dJR_a*yb')'  ;  (d_R_b*yb' ) 7  ; 

( d_R_c  *yb '  )  '  ;  (d_R__d*yb'  )  '  ]  '  ; 

%  Rotate  the  measured  vector  using  the  quaternion  guess 
%  and  calculate  the  error  between  real  value  and  rotated  one 
M_hat  =  [dA2+aA2-bA2-cA2  2*  (a*b-c*d)  2*(a*c+b*d); 

2*  (a*b+c*d)  dA2+bA2-cA2-aA2  2* (b*c-a*d)  ; 

2*  ( a*c-b*d)  2*  (b*c+a*d)  dA2+cA2-aA2-bA2 ]  ; 

R_hat  =  [M_hat  zeros (3)  ;  zeros (3)  M_hat] ; 

err  =  ye  -  R_hat*yb' ; 

%  Calculate  next  quaternion  using  previous  one  and  the  Jacobian 
n_hat  =  n_hat  -  inv ( jacobian7 * jacobian) * jacobian ' *err; 

end 

njiat  =  n_hat/ (sqrt (n_hat 7 *n_hat) ) ; 

res  =  [res  ;  round(theta*180/pi)  n  n_guess'  njiat ' ] ; 
end 

result  =  [res ( :  ,  1 : 2 )  res(:,10)  res(:,3)  res(:,ll)  res(:;4)  res{:,12) 
res (  :  ,  5 )  res ( : , 13 ) ] ; 

%  Write  the  results  to  an  excel  file,  which  must  be  opened 
channel  =  ddeinit { 7  excel ' ,  ' Converge . xls ' ) ; 

rc  =  ddepoke (channel , 'r2cl:rl9c9' ,  result); 
rc  =  ddeterm( channel) ; 
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A2.  NEWTON-METHOD 

% 

%  In  order  to  use  the  Newton  Method  the  only  change  is  to  replace 
%  the  part  inside  the  loop  with  the  code  below 
% 

ac_n  =  [ac_n  n_hat] ; 

a=n_hat ( 1 ) ;  b=n_hat ( 2 ) ;  c=n_hat ( 3 ) ;  d=n_hat ( 4 )  ; 

dM_a2  =  [  2  0  0  ;  0  -2  0  ;  0  0  -2]  ; 
dR_a2  =  [dM_a2  zeros (3 ); zeros (3 )  dM_a2]; 
dM_b2  =  [-2  0  0  ;  0  2  0  ;  0  0  -2]; 

dR_b2  =  [dM_b2  zeros (3 ); zeros (3 )  dM_b2] ; 
dM_c2  =  [-2  0  0  ;  0  -2  0  ;  0  0  2] ; 

dR_c2  =  [dM_c2  zeros (3 ); zeros (3)  dM_c2] ; 
dM_d2  =  [  2  0  0  ;  0  2  0  ;  0  0  2  ]  ; 

dR_d2  =  [dM_d2  zeros (3 ); zeros (3)  dM_d2] ; 

dM_ab  =  [0  2  0  ;  2  0  0  ;  0  0  0]  ; 

dR_ab  =  [dM_ab  zeros {3 ); zeros (3)  dM_ab] ; 
dM_ac  =  [0  0  2  ;  0  0  0  ;  2  0  0]  ; 

dR_ac  =  [dM_ac  zeros (3 ); zeros (3)  dM_ac] ; 
dM_ad  =  [0  0  0  ;  0  0  -2  ;  0  2  0]  ; 

dR_ad  =  [dM_ad  zeros (3 ); zeros (3 )  dM_ad] ; 
dM_bc  =  [0  0  0  ;  0  0  2  ;  0  2  0]; 

dR_bc  =  [dM_bc  zeros (3) ; zeros (3)  dM_bc] ; 
dM_bd  =  [0  0  2  ;  0  0  0  ;  -2  0  0] ; 

dR_bd  =  [dM_bd  zeros (3) ; zeros (3)  dM_bd] ; 
dM_cd  =  [0  -2  0  ;  2  0  0  ;  0  0  0]  ; 

dR_cd  =  [dM_cd  zeros ( 3 ) ; zeros ( 3 )  dM_cd] ; 

auxl  =  [yb ' *  dR_a2  *  err  yb' *dR_ab*err  yb' *dR_ac*err  yb' *dR_ad*err; 

yb' *dR_ab*err  yb' *dR_b2*err  yb' *dR_bc*err  yb' *dR_bd*err; 
yb ' *dR_ac*err  yb' *dR_ab*err  yb' *dR_c2*err  yb' *dR_cd*err; 
yb ' *dR_ad*err  yb' *dR_bd*err  yb' *dR_cd*err  yb' *dR_d2*err] 

R_a  =  [  a  b  c  ;  b  -a  -d  ;  c  d  -a] ; 
d_R_a  =  2*[R_a  zeros (3)  ;  zeros (3)  R_a] ; 

R_b  =  [-b  a  d  ;  a  b  c  ;  -d  c  -b] ; 
d_R_b  =  2 * [ R_b  zeros (3)  ;  zeros (3)  R_b] ; 

R_c  =  [-c  -d  a  ?  d  -c  b  ;  a  b  c]  ; 
d_R_c  =  2*[R_c  zeros (3)  ;  zeros (3)  R_c] ; 

R_d  =  [  d  -c  b  ;  c  d  -a  ;  -b  a  d] ; 
d_R_d  =  2*[R_d  zeros (3)  ;  zeros (3)  R_d] ; 

aux2  =  [ (d_R_a*yb) '  ;  (d_R_b*yb) '  ; (d_R_c*yb) '  ; (d_R_d*yb) '  ] ; 

%  Rotate  the  measured  vector  using  the  quaternion  guess 
%  and  calculate  the  error  between  real  value  and  rotated  one 
M_hat  =  [d^2+a^2-b^2-c~2  2*(a*b-c*d)  2*(a*c+b*d); 

2*  (a*b+c*d)  dA2+bA2-cA2-a/S2  2*  (b*c-a*d)  ; 

2* (a*c-b*d)  2* (b*c+a*d)  dA2+cA2-aA2-bA2] ; 
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R_hat  =  [M_hat  zeros (3)  ;  zeros (3)  M_hat] ; 

err  =  ye  -  R_hat*yb' ; 

%  Calculate  next  quaternion  using  previous  one,  the  gradient 
%  and  the  Hessian 

grad  =  -2 *aux2*err ; 

hess  =  -2  * (auxl-aux2*aux2 ' ) ; 

n_hat  =  n_hat  -  inv (hess) *grad; 
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APPENDIX  B  -  EXTENDED  KALMAN  FILTER 


Bl.  MAIN  PROGRAM 

%  This  is  the  main  program  of  the  Extended  Kalman  filter 
%  designed  for  estimating  attitude  of  a  body.  The  attitude 
%  parameters  are  quaternions . 

% 

%  The  measurements  should  be  genrated  in  advance  or  collected 
%  in  real  experiment  and  should  be  written  to  an  excel  file. 

%  This  file  must  be  opened  when  the  filter  runs. 

% 

%  This  program  calls  three  other  routines: 

%  -  converg  :  the  convergence  algorithm,  can  be  Newton  or 

%  Gaus s -Newton; 

%  -  transition  :  allows  the  projection  ahead  through  the 

%  linearization  of  the  state  transition  matrix; 

%  -  projection  :  allows  the  projection  ahead  using  the  0DE45 

%  integration. 

% 

%  By  Joao  Luis  Marins,  Monterey,  July  2000. 

% 

clc;  clear; 

%  Reading  measurements  (p, q, r , gx, gy, gz,hx, hy,hz) 

%  The  data  is  read  from  an  excel  file,  which  must  be  opened 
sim_data  =  []; 

channel  =  ddeinit (' excel ' ,  'dynamic_2.xls'); 
sim__data  =  ddereq (channel ,  1  rlcl :  r2400c9  ')  ; 
rc  =  ddeterm( channel)  ; 

%  Definitions 

run_time  =  4.50;  %  simulation  time  in  minutes 

T  =  0.125;  %  resolution  time  in  seconds 

samp  =  run_t ime * 6 0 / T ;  %  number  of  samples 

%  Time  constants  used  indide  the  routines  transition  and  projection 
tau_rx  =  0.002; 
tau_ry  =  0.15; 
tau_rz  =  1.0; 

%  Values  of  std  dev  used  in  the  Q  matrix  -  E[w’*w] 
stdl  =  le-7  ; 

std2  =0.01;  . 

std3  =  0.30; 

%  Process  model  -  noise  vector  for  angular  rates 

randn ( ' state ' , sum( 100*clock) ) ; 

wrx  =  stdl*randn ( 1 , samp) ; 

wry  =  std2*randn(l, samp) ; 

wrz  =  std3*randn(l, samp) ; 
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%  Process  model  -  noise  vector  for  quaternions  -  no  noise 

wna  =  zeros ( 1 , samp) ; 

wnb  =  zeros ( 1 , samp) ; 

wnc  =  zeros (1, samp) ; 

wnd  =  zeros (1 , samp) ; 

%  System  noise  vector 

w  =  [  wrx  ;  wry  ;  wrz  ;  wna  ;  wnb  ;  wnc  ;  wnd  ] ; 

%  Measurements  -  noise  vector  for  angular  rates 
vrx  =  randn ( 1 , samp) ; 
vry  =  randn ( 1 , samp) ; 
vrz  =  randn ( 1 ,  samp) ; 

%  Measurements  -  noise  vector  for  quaternions 

vna  =  randn (1 , samp) ; 

vnb  =  randn ( 1 , samp ) ; 

vnc  =  randn ( 1 , samp ) ; 

vnd  =  randn (1, samp) ; 

%  Measurement  noise  vector 

v  =  [  vrx  ;  vry  ;  vrz  ;  vna  ;  vnb  ;  vnc  ;  vnd  ] ; 

%  Step  0  :  Initial  conditions 

x_hat_minus  =  [  0.01  ;  0.01  ;  0.01  ;  0.5  ;  0.5  ;  0.5  ;  0.5  ]; 

%  Initial  covariance  matrix 

P_minus  =  diag ( [  0.5  0.5  0.5  0.5  0.5  0.5  0.5]); 

%  Matrices  for  flag  =  0  (quaternions  do  not  converge) 

R_0  =  diag ( [ ( 7 . 3 6e-5 ) ^2  (0.015)^2  (0.029)^2]); 

H_0  =  [eye (3)  zeros (3,4)]; 

%  Matrices  for  flag  =  1  (quaternions  converge) 

R_aux  =  diag ([ (0.110^2)  (0.167A2)  ( 0 . 045^2 )  ( 0 . 03 A2 )  (  0 . 03 A2 )  (0.03^2)]) 

H_1  =  eye ( 7 ) ; 

%  Process  noise  covariance  matrix 
Q  =  diag ( [  stdlA2  std2"2  std3A2  0000]); 

x_hat__plus_cum  =  [x_hat_minus]  ; 

%  Accximulate  all  the  estimates 
n_acum  =  [ ] ; 

%  Registry  number  of  times  the  convergence  algorithm  diverges 
count_flag  =  0; 
f  lag_cum  =  [  ]  ; 

for  k  =  2: samp 

%  Step  1  :  Read  sensors  (angular  rates,  gravity,  and  magnetic  field) 
ye  =  [0  ;  0  ;  -1  ;  -0.5  ;  0  ;  sqrt{3)/2  ]; 

g_norm  =  (sim_data (k, 4 : 6) ) ' /sqrt ( (sim_data (k, 4:6))* (sim_data (k, 4:6))') 
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h_norm  =  (sim_data (k, 7 : 9) ) '/sgrt ( (sim_data (k, 7 : 9 ) ) * (sim_data (k, 7 : 9 ) ) ' ) 

yb  =  [  g_norm  ;  h_norm  ] ; 

%  First  guess  is  the  current  quaternion 
guess  =  x_hat_minus (4:7)  ; 

%  Step  2  :  Converge  the  quaternions  using  x_hat(k|k-l) 

[n, flag, A]  =  Converg (ye, yb, guess, k) ; 
flag_cum  =  [flag_cum  flag]  ; 

P_minus  =  diag ([  0.5  0.5  0.5  0.5  0.5  0.5  0.5]); 
n_acum  =  [n_acum  n]  ; 

if  flag  ==  1  %  quaternion  converge 

z  =  [ (sim_data(k,l:3) ) '  ;  n] ; 

%  The  A  matrix  relates  the  covariance  of  the  measurement 
%  noise  (6  by  6)  to  the  covariance  of  the  quaternions 
%  after  convergence  (4  by  4) 

R_1  =  [  R_0  zeros (3, 4)  ;  zeros (4, 3)  A*R_aux*A'  ]; 

%  Step  3  :  Determine  the  Kalman  gain 
K  =  P_minus*H_JL  '  *inv  (H_l*P_minus*H_l #  +  R_l)  ; 

%  Step  4  :  Update  estimate  with  measurement 
x_hat_plus  =  x_hat_minus  +  K*[z  -  H_l*x_hat_minus]  ; 

%  Step  5  :  Compute  error  covariance  for  updated  estimate 
P _ plus  =  (eye (7)  -  K*H_1 ) *P_minus ; 

else  %  quaternion  diverge 

z  =  [ (sim_data (k, 1 : 3  )  )  1  ]  ; 

%  Step  3  :  Determine  the  Kalman  gain 
K  =  P_minus *H_0  '  *  inv { H_0 * P_minus *H_0  '  +  R_0 )  ; 

%  Step  4  :  Update  estimate  with  measurement 
x_hat_plus  =  x_hat_minus  +  K*[z  -  H_0*x_hat_minus]  ; 

%  Step  5  :  Compute  error  covariance  for  updated  estimate 
P  plus  =  (eye (7)  -  K*H_0 ) *P_minus ; 

count_flag  =  count_flag  +  1? 

end 

x_hat_plus_cura  =  [x_hat_plus_cum  x_hat_plus]  ; 

%  Step  6  :  Project  ahead 
phi  =  transition (x_hat_plus)  ; 

P_minus  =  phi*P_jplus*phi '  +  Q; 

[ti  ,  x_aux]  =  ode45  ( 'projection' ,  [0  T]  , x_Jhat_plus )  ; 
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x_hat__minus  =  (x_aux  (  f  ind  ( ti==T)  ,:))'; 


end 

m  =  length (x_hat_plus_cum ( 1 , :  )  )  ; 
figure (1) ; 

subplot (311) ;plot ( (1 :m)  *0 . 125 , x_hat_plus_cum ( 1 , : ) , '  b' ) ; 
set  (gca, ' LineWidth' ,1.5, ' FontSize ' , 12 ) ; hold; 

title ( 'Covergence  of  Angular  Rates');  ylabel('roll  (rad/sec)'); 
subplot ( 312 ) ; plot ( ( 1 :m) *0.125, x_hat_plus_cum  (2 , : ) , ' b' ) ; 
set  (gca, ' LineWidth' ,1.5, ' FontSize ' , 12) ;hold; 
ylabel ( 'pitch  (rad/sec) ' ) ; 

subplot  (313  )  ; plot  (  ( 1  :m)  *0 . 125  ,  x_hat_plus_cum (3  ,  :  )  ,  'b'  )  ; 
set  (gca, ' LineWidth' ,1.5, ' FontSize' , 12 ) ;hold; 
ylabel ('yaw  (rad/sec)');  xlabel('time  (sec)'); 

figure (2 ) ; 

subplot  (411)  ;  plot  (  (1  :m)  *0 . 125  ,  x_hat_jplus_cum (4  ,  : )  ,  'b'  )  ; 
set  (gca, ' LineWidth' ,1.5, ' FontSize' , 12 ) ;hold; 

title (' Covergence  of  Quaternion  Components  -  ai  +  bj  +  ck  +  d'); 
ylabel ('a'); 

subplot (412) ; plot ( (l:m) *0 . 125 , x_hat_plus_cum { 5 , : ) , 'b' ) ; 
set  (gca, ' LineWidth' ,1.5, ' FontSize' , 12 ) ;hold; 
ylabel ( ' b ' ) ; 

subplot (413 ) ;plot ( (1 :m) *0 . 125 , x_hat_plus_cum ( 6 , : ) , 'b' ) ; 
set  (gca, 'LineWidth' ,1.5, 'FontSize' , 12) ;hold; 
ylabel ( ' c ' ) ; 

subplot  (414)  ; plot  (  (l:m)  *0 . 125 ,  x__hat_plus_cum{7  ,  :  )  ,  'b'  )  ; 
set  (gca, 'LineWidth' ,1.5, 'FontSize' , 12) ;hold; 
xlabel ( ' time  ( sec ) ' ) ; 
ylabel ( ' d' ) ; 
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B2.  CONVERGENCE  ROUTINE 


function  [n, flag, A]  =  converg (ye, yb, guess, k) 

This  function  uses  the  Gauss-Newton  method  to  find  the  best 
quaternion  that  rotates  the  measurements  of  gravity  and  magnetic • 
field  from  body  coordinates  to  earth  coordinates .  The  first  guess 
is  the  value  calculated  from  the  state  equations .  The  calculated 
values  are  normalized  at  each  iteration. 

The  Gauss-Newton  is  a  first  order  approximation  where  the 
Jacobian  is  used  instead  of  the  gradient  and  hessian  of  the 
Newton  method. 

The  flag  indicates  if  the  quaternion  converged  to  a  value  close 
to  the  initial  guess  or  not  (1  or  0) .  The  threshold  can  be 
adjusted. 

By  Joao  Luis  Marins,  Monterey,  June  2000. 


n_hat  =  guess; 
ac_n  =  [ ] ; 

for  i=l : 1 

%  Normalize  the  quaternion 

n_hat  =  n_hat/ (sqrt (n_hat ' *n_hat) ) ; 
ac_n  =  [ac_n  n_hat] ; 

a=n_hat ( 1 ) ;  b=n_hat ( 2 ) ;  c=n_hat ( 3 ) ;  d=n_hat ( 4 )  ; 

%  Calculate  the  partial  derivatives  needed  for  the  Jacobian 
R_a  =  [  a  b  c  ;  b  -a  -d  ;  c  d  -a]  ; 
d_R_a  =  2 * [ R_a  zeros (3)  ;  zeros (3)  R_a] ; 

R_b  =  t-b  a  d  ;  a  b  c  ;  -d  c  -b] ; 
d_R_b  =  2 * [ R_b  zeros (3)  ;  zeros (3)  R_b] ; 

R_c  =  [ — c  — d  a  ;  d  -c  b  ;  a  b  c] ; 
d_R_c  =  2 * [ R_c  zeros (3)  ;  zeros (3)  R_c] ; 

R_d  =  [  d  -c  b  ;  c  d  -a  ;  -b  a  d] ; 
d_R_d  =  2 * [ R_d  zeros (3)  ;  zeros (3)  R_d] ; 

jacobian  =  -[  (d_R_a*yb')'  ;  (d_R_b*yb')'  ; 

( d_R_c *yb 1 ) ’  ;  ( d_R_d*yb 

%  Rotate  the  measured  vector  using  the  quaternion  guess 
%  and  calculate  the  error  between  real  value  and  rotated  one 
M_hat  =  [dA2+a~2-b'v2-c'12  2*(a*b-c*d)  2*(a*c+b*d); 

2*  (a*b+c*d)  d/'2+b'v2-c/'2-a/'2  2*(b*c-a*d); 

2* (a*c-b*d)  2* (b*c+a*d)  d^2+c^2-a^2-b^2] ; 

R_hat  =  [M_hat  zeros (3)  ;  zeros (3)  M_hat] ; 

err  =  ye  -  R_hat*yb ' ; 
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%  Calculate  next  quaternion  using  previous  one  and  the  Jacobian 
n_hat  =  n_hat  -  inv{ jacobian' * jacobian) * jacobian' *err ; 

end 

n_hat  =  n_hat/  {sqrt  (n_hat '  *n_hat)  )  ; 

%  Calculate  the  matrix  that  converts  the  measurement  errors 

%  of  the  six  sensors  into  the  calculated  measurement  error 

%  of  the  four  computed  quaternions 

A  =  inv(  jacobian'  *jacobian)  *  jacobian7  *R_hat; 

%  Check  if  the  algorithm  converges 

distance  =  sqrt ( sum( (n_hat-guess ) . ^2 )  ) ; 
n  =  n_hat; 

if  (k  ==  2) 

flag  =  1; 

elseif  (distance  <=  0.5) 
flag  =  1; 
else 

flag  =  0; 

end 
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B3.  LINEARIZATION  OF  THE  STATE  TRANSITION  MATRIX 

function  xprime  =  projection(t,x) ; 

%  This  function  is  an  ODE  file  with  the  differential  equations 

%  describing  the  state  equations  for  the  filter.  The  filter  calls 

%  this  routine  using  The  0DE45  function  when  projecting  ahead 

%  (calculating  x_hat_rainus ) . 

% 

%  By  Joao  Luis  Marins,  Monterey,  July  2000. 

% 

%  time  constants  for  angular  rates 

tau_rx  =  0.002; 

tau_ry  =  0.15; 

tau_rz  =  1.00; 

xprime  =  [ -x ( 1 ) / tau_rx ; 

-x(2) / tau_ry ; 

-x(3 ) /tau_rz; 

(  x(3) *x(5)-x(2) *x(6)+x(l)*x(7) ) / 

/  (2*sqrt (x(4),'2+x(5)^2+x(6)/'2+x(6)/'2)  )  ; 

(-  x ( 3 ) *x ( 4 ) +x ( 1 ) *x ( 6 ) +x { 2 ) *x ( 7 ) ) / 

/  (2*sqrt  (x(4)~2+x(5)/'2+x(6)/'2+x(6)A2)  )  ; 

(  x ( 2 ) *x ( 4 ) -x ( 1 ) *x ( 5 ) +x ( 3 ) *x ( 7 ) ) / 

(2*sqrt  (x(4)  /'2+x(5)  A2+x(6)  A2+x(6)  *2)  )  ; 

( -x  ( 1)  *x  (4 )  -x  (2 )  *x( 5)  -x  (3  )  *x  ( 6) )  / 

(2*sqrt(x(4)/'2+x(5)^2+x(6)  A2+x(6)~2)  )  ]  ; 
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B4.  PROJECT  AHEAD  USING  ODE45 


function  [phi]  =  transition (x„hat_plus ) 

%  This  function  calculates  the  state  transition  matrix  phi, 

%  which  is  the  linearization  of  the  F  matrix. 

%  The  filter  calls  this  routine  when  projecting  ahead 

%  (calculating  P_minus) . 

o. 


%  resolution  time  in  seconds 

T  =  0.01; 

%  time  constants  for  angular  rates 

tau_rx  =  0.002; 
tau_ry  =  0.15; 
tau_jrz  =  1.0; 

xl  =  x_hat_plus (1) ; 
x2  =  x_hat_plus  (2)  ? 
x3  =  x_hat__plus  (3)  ; 
x4  =  x_hat_plus (4) ; 
x5  =  x_hat  __plus (5) ; 
x6  =  x_hat_plus  (6)  ; 
x7  =  x_hat_plus (7) ; 

n2  =  sqrt(x4A2  +  x5A2  +  x6A2  +  x7A2); 

fl  =  -xl/tau_rx; 
f2  =  -x2/tau_ry; 
f3  =  -x3/tau_rz; 

f 4  =  0.5*  (  x3 *x5  -  x2*x6  +  xl*x7)/n2; 
f 5  =  0.5*(-x3*x4  +  xl*x6  +  x2*x7)/n2; 
f6  =  0.5* (  x2*x4  -  xl*x5  +  x3*x7)/n2; 
f 7  =  0 . 5* ( -xl*x4  -  x2*x5  -  x3*x6)/n2; 

%  Derivatives  for  linearization 


dfl_xl 

= 

1  -  T/tau_rx; 

df  2_x2 

= 

1  -  T/tau_ry; 

df 3_x3 

= 

1  -  T/tau_rz; 

df 4_xl 

= 

0.5* (  x7/n2) ; 

df 4_x2 

= 

0 . 5* ( -x6/n2 ) ; 

df 4_x3 

= 

0.5* (  x5/n2 ) ; 

df 4_x4 

= 

1  +  0.5*  ( -* f 4 * 

x4/n2 A3 ) ; 

df 4_x5 

= 

0.5* (  x3/n2  - 

f4*x5/n2~3) 

df 4_x6 

- 

0.5*(-x2/n2  - 

f4*x6/n2A3 ) 

df 4_x7 

= 

0.5* (  xl/n2  - 

f4*x7/n2/'3 ) 

df 5_xl 

= 

0.5* (  x6/n2 ) ? 

df 5_x2 

= 

0.5* (  x7/n2) ; 

df  5_x3 

= 

0.5* ( -x4/n2 ) ; 

df 5_x4 

= 

0.5* (-x3 /n2  - 

f5*x4/n2^3 ) 
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df 5_x5  =  1  +  0.5* (-f5*x5/n2A3 ) ; 
df 5_x6  =  0.5*  (  xl/n2  -  f5*x6/n2A3); 
df5_x7  =  0.5* (  x2/n2  -  f5*x7/n2A3) ; 

df6_xl  =  0.5* (-x5/n2) ; 

df 6_x2  =  0.5* (  x4/n2) ; 

df 6_x3  =  0.5* (  x7/n2); 

df 6_x4  =  0 . 5* {  x2/n2  -  f6*x4/n2A3) ; 

df6_x5  =  0.5*(-xl/n2  -  f6*x5/n2A3); 

df 6_x6  =  1  +  0.5* (-f6*x6/n2A3) ; 

df 6_x7  =  0.5* (  x3/n2  -  f 6*x7/n2A3 ) ; 

df7_xl  =  0.5* (-x4/n2) ; 

df7_x2  =  0.5* (~x5/n2) ; 

df 7_x3  =  0.5* (-x6/n2) ; 

df7_x4  =  0.5*(-xl/n2  -  f7*x4/n2A3); 

df 7_x5  =  0.5*  (-x2/n2  -  f7*x5/n2A3); 

df  7_x6  =  0.5*  (-f7*x7 /n2A3 )  ; 

df 7_x7  =  1  +  0.5* (-x3/n2  -  f7*x6/n2A3); 


[  df l_xl 

0 

0 

0 

0 

0 

0 

0 

df 2_x2 

0 

0 

0 

0 

0 

0 

0 

df 3_x3 

0 

0 

0 

0 

df  4_xl 

df  4_jx2 

df  4__x3 

df  4__x4 

df  4__x5 

df 4_x6 

df  4. 

_x7 

df 5_xl 

df  5__x2 

df 5_x3 

df 5_x4 

df 5_x5 

df  5_x6 

df  5. 

_x7 

df 6_xl 

df  6_x2 

df  6_x3 

df  6_x4 

df  6_x5 

df 6_x6 

df  6. 

_x7 

df 7_xl 

df  7_x2 

df  7_x3 

df 7_x4 

df 7_x5 

df  7_jx6 

df  7. 

_x7 
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APPENDIX  C  -  DATA  GENERATION  FOR  SIMULATION 


Cl.  MAIN  PROGRAM 

%  This  program  generates  measured  values  of  gravity  and  local 
%  magnetic  field  by  integrating  Euler  equations  with  a  profile  for 
%  the  angular  rates . 

%  The  Euler  angles  obtained  from  the  euler  equations  are 
%  used  to  make  up  a  direct  cosine  matrix  and  then  to  rotate 
%  values  in  earth  frame  into  body  frame. 

%  The  measurement  noise  variance  can  be  chosen. 

%  The  final  values  are  written  to  a  excel  file  so  that  they  can  be 
%  read  later  by  the  Extended  Kalman  filter. 

% 

%  It  calls  the  routine  gene_euler  which  contains  the  Euler  equations 
%  to  be  used  by  the  ODE45  integration  routine. 

% 

%  By  Joao  Luis  Marins,  Monterey,  May  2000. 

% 


clear;  clc; 
x  =  [  ]  ; 

%  Creating  angular  rates  profile  (p,q,r) 

pi  =  [zeros (100, 1)  ;  0 . 02*ones ( 629 , 1)  ;  zeros  ( 500 , 1 )] ; 

ql  =  [zeros (1229 , 1) ] ; 
rl  =  [zeros (1229 , 1) ]  ; 

p2  =  [zeros (1229 , 1) ] ; 

q2  =  [  zeros ( 100 , 1 )  ;  0 . 02*ones ( 629 , 1)  ;  zeros ( 500 , 1 )] ; 

r2  =  [zeros (1229 , 1) ] ; 

sim_pqr  =  [pi  ql  rl  ;  p2  q2  r2] ; 

%  Define  the  initial  values  for  the  Euler  angles 
xO  =  [0  0  0] ; 

%  Integrate  Euler  equations 
for  i=l : 2400 

[ t 1 , xl ]  =  ode45 ( 1 gene_euler ' ,  [0;0.125],[  xO sim_pqr ( i ; 

[a, b]  =  size (xl) ; 
xO  =  xl (a, 1 : 3 ) ; 
x  =  [x  ;  xO] ; 

end 

angles  =  x; 

%  Gravity  and  local  magnetic  field  in  earth  frame 
gO  =  [0  ;  0  ;  -1];  h0  =  [-0.5  ;  0  ;  sqrt(3}/2]; 

result  =  [ ] ; 
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%  Calculate  the  direct  cosine  matrix  to  rotate  the  vector  from 
%  earth  frame  into  body  frame 

for  i-1 : 2400 

phi  =  angles (i,l);  theta  =  angles (i, 2);  psi  =  angles (i, 3) 

C_phi  =  [  1  0  0 

0  cos (phi)  sin (phi)  ; 

0  -sin(phi)  cos (phi)  ]; 

C_theta  =  [  cos (theta)  0  -sin (theta)  ; 

0  1  0 

sin(theta)  0  cos (theta)  ]; 

C_psi  =  [  cos (psi)  sin(psi)  0  ; 

-sin(psi)  cos (psi)  0  ; 

0  0  1  ]; 

%  Rotating  the  vectors 

gl  =  C_phi*C_theta*C_psi*gO; 
hi  =  C_phi*C_theta*C_psi*hO ; 

result  =  [result  ;  [  gl '  hi'  ]]; 

end 

%  Adding  measurement  noise 

meas  =  [ sim_pqr ( 1 : 2400 , : )  result ( 1 : 2400 ,:)] ; 

noise  =  randn ( 2400 , 9 ) ; 

noise(:,l)  =  2e-3*noise ( : , 1) ; 
noised, 2)  =  2e-3 *noise ( : , 2 ) ; 
noise ( : , 3 )  =  2e-3 *noise ( : , 3 ) ; 
noise ( : , 4 )  =  0 . 02*noise ( : , 4 ) ; 
noise(:,5)  =  0 . 02*noise ( : , 5 ) ; 
noise ( : , 6 )  =  0 . 02*noise ( : , 6 ) ; 
noise(:,7:9)  =  0 . 02*noise ( : , 7 : 9 ) ; 

meas_noise  =  meas  +  noise; 

%  Writing  data  to  an  excel  file 

channel  =  ddeinit ( ' excel ' ,  ' dynamic_2 . xls ' ) ; 

rc  =  ddepoke (channel, ' rlcl : r2400c9 ' ,  meas_noise)  ; 

rc  =  ddeterm( channel) ; 

figured)  ;plot  (  (1:2400)  /  8,  angles  (1:2400,  :))  ; 
figure (2) ;plot ((1:2400) / 8 , sim_pqr ( 1 : 2400 , : ) ) ; 
figure (3) ;plot( (1:2400) / 8 , result ( 1 : 2400 , : ) ) ; 


C2.  EULER  EQUATIONS 


function'  xprime  =  gene_eulerl  ( t ,  x)  ; 

%  This  function  is  an  ODE  file  with  the  Euler  equations. 

% 

%  The  main  program  calls  this  routine  using  the  ODE45  function 
%  in  order  to  integrate  the  Euler  equations  to  find  the  three 
%  Euler  angles . 

g. 

*o 


%  By  Joao  Luis  Marins,  Monterey,  May  2000. 


%  Euler  equations 

xprime  =  [x(4)  +  x(5) *sin(x(l) ) *tan(x(2) )  +  x(6) *cos (x (1) ) *tan (x (2) ) ; 
x(5) *cos (x(l) )  -  x(6)*sin(x(l)); 

x(5) * (sin(x(l) ) /cos (x(2) ) )  +  x(6) * (cos (x ( 1) ) /cos(x(2) ) ) ; 

0; 

0; 

0]  ; 
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